rotation controller base. added new bin for figuring out quaternions. delete later? add new inputs and modules

Cargo.lock

@@ -34,6 +34,12 @@ version = "0.2.21"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923"
 
+[[package]]
+name = "android-tzdata"
+version = "0.1.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0"
+
 [[package]]
 name = "android_system_properties"
 version = "0.1.5"
@@ -180,14 +186,15 @@ checksum = "613afe47fcd5fac7ccf1db93babcb082c5994d996f20b8b159f2ad1658eb5724"
 
 [[package]]
 name = "chrono"
-version = "0.4.43"
+version = "0.4.39"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fac4744fb15ae8337dc853fee7fb3f4e48c0fbaa23d0afe49c447b4fab126118"
+checksum = "7e36cc9d416881d2e24f9a963be5fb1cd90966419ac844274161d10488b3e825"
 dependencies = [
+ "android-tzdata",
  "iana-time-zone",
  "num-traits",
  "serde",
- "windows-link",
+ "windows-targets",
 ]
 
 [[package]]

Cargo.toml

@@ -20,7 +20,6 @@ csv = "1.4.0"
 serde_with = "3"
 serde_json = "1.0.149"
 
-
 clap = { version = "4", features = ["derive"] }
 
 
@@ -33,3 +32,8 @@ opt-level = 3
 [[bin]]
 name = "record"
 path = "src/main_record.rs"
+
+
+[[bin]]
+name = "test"
+path = "src/main_testing.rs"

analyze.py

@@ -1,3 +1,4 @@
+import json
 import tkinter as tk
 from pathlib import Path
 from tkinter import messagebox, ttk
@@ -10,10 +11,8 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolb
 class SimVisualizer:
     def __init__(self, root: tk.Tk):
         self.root = root
-        self.root.title("Simulation Data Viewer")
+        self.root.title("Simulation Data Viewer (JSON Format)")
 
-        # --- Protocol Handler ---
-        # This handles the "X" button on the window frame
         self.root.protocol("WM_DELETE_WINDOW", self.quit_app)
 
         self.results_dir = Path("results")
@@ -24,16 +23,11 @@ class SimVisualizer:
         self.refresh_file_list()
 
     def setup_ui(self):
-        # 1. Create a PanedWindow (Horizontal orientation)
         self.paned_window = ttk.PanedWindow(self.root, orient=tk.HORIZONTAL)
         self.paned_window.pack(fill=tk.BOTH, expand=True)
 
-        # --- Sidebar Frame ---
+        # --- Sidebar ---
-        # Note: We still define a width, but it's now the *initial* width
         sidebar = ttk.Frame(self.paned_window, width=300, padding="10")
-
-        # 2. Add the sidebar to the PanedWindow
-        # 'weight=0' keeps it from growing automatically when the window is resized
         self.paned_window.add(sidebar, weight=0)
 
         ttk.Label(sidebar, text="Result Files", font=("Helvetica", 12, "bold")).pack(
@@ -59,7 +53,6 @@ class SimVisualizer:
                 command=self.update_plot,
             ).pack(anchor=tk.W)
 
-        # --- Quit Button ---
         spacer = ttk.Label(sidebar, text="")
         spacer.pack(fill=tk.Y, expand=True)
 
@@ -70,9 +63,6 @@ class SimVisualizer:
 
         # --- Plot Frame ---
         self.plot_frame = ttk.Frame(self.paned_window, padding="10")
-
-        # 3. Add the plot frame to the PanedWindow
-        # 'weight=1' ensures the plot area expands to take up all remaining space
         self.paned_window.add(self.plot_frame, weight=1)
 
         self.fig, self.ax = plt.subplots(figsize=(8, 6))
@@ -83,7 +73,6 @@ class SimVisualizer:
         self.toolbar.update()
 
     def quit_app(self):
-        """Cleanly close the plots and the application."""
         plt.close("all")
         self.root.destroy()
         self.root.quit()
@@ -92,9 +81,16 @@ class SimVisualizer:
         if not self.results_dir.exists():
             self.results_dir.mkdir(parents=True)
 
-        csv_files = sorted(self.results_dir.glob("*.csv"))
+        # Updated to look for .json or .log files
+        log_files = sorted(
+            [
+                f
+                for f in self.results_dir.glob("*")
+                if f.suffix in [".json", ".log", ".txt"]
+            ]
+        )
         self.file_listbox.delete(0, tk.END)
-        for f in csv_files:
+        for f in log_files:
             self.file_listbox.insert(tk.END, f.name)
 
     def on_file_select(self, event):
@@ -103,42 +99,74 @@ class SimVisualizer:
             filename = self.file_listbox.get(selection[0])
             self.current_file = self.results_dir / filename
             try:
-                self.df = pd.read_csv(self.current_file)
+                # Use Pandas' built-in fast JSON engine
+                # 'lines=True' treats each line as a JSON object
+                self.df = pd.read_json(self.current_file, lines=True)
+
+                # Check if the columns are nested and flatten them if necessary
+                # If read_json doesn't flatten automatically, we expand the dicts:
+                for col in [
+                    "angvel_target",
+                    "angvel_current",
+                    "mot_current",
+                    "mot_target",
+                    "rot_target",
+                    "rot_current",
+                ]:
+                    if col in self.df.columns:
+                        # This expands the dict column into new columns (col.x, col.y, col.z)
+                        expanded = pd.json_normalize(self.df[col])
+                        expanded.columns = [f"{col}.{c}" for c in expanded.columns]
+                        self.df = pd.concat(
+                            [self.df.drop(columns=[col]), expanded], axis=1
+                        )
+
                 self.update_plot()
             except Exception as e:
-                messagebox.showerror("Error", f"Could not load file: {e}")
+                messagebox.showerror("Error", f"Could not load JSON file: {e}")
 
     def update_plot(self):
         if self.df is None:
             return
 
-        coord = self.axis_var.get()
+        c = self.axis_var.get()  # current axis (x, y, or z)
         self.ax.clear()
 
+        # Mapping new nested keys to the plot
+        # The keys follow the pattern: category.axis
         self.ax.plot(
-            self.df["time"], self.df[f"target_{coord}"], label=f"target_{coord}"
+            self.df["time"], self.df[f"rot_target.{c}"], label=f"Rot Target {c}"
         )
         self.ax.plot(
-            self.df["time"], self.df[f"current_{coord}"], label=f"current_{coord}"
+            self.df["time"], self.df[f"rot_current.{c}"], label=f"Rot Current {c}"
+        )
+
+        self.ax.plot(
+            self.df["time"], self.df[f"angvel_target.{c}"], label=f"AngVel Target {c}"
         )
         self.ax.plot(
-            self.df["time"], self.df[f"mot_{coord}"].clip(-1, 1), label=f"mot_{coord}"
+            self.df["time"], self.df[f"angvel_current.{c}"], label=f"AngVel Current {c}"
+        )
+
+        self.ax.plot(
+            self.df["time"],
+            self.df[f"mot_target.{c}"],
+            label=f"Mot Target {c}",
+            linestyle="--",
+        )
+        self.ax.plot(
+            self.df["time"],
+            self.df[f"mot_current.{c}"],
+            label=f"Mot Current {c}",
+            alpha=0.7,
         )
-        # self.ax.plot(
-        #     self.df["time"],
-        #     self.df[f"dmot_{coord}"].clip(-1, 1),
-        #     label=f"desired mot_{coord}",
-        #     linestyle="--",
-        # )
 
         self.ax.set_xlabel("Time (s)")
         self.ax.set_ylabel("Value")
-        self.ax.set_title(f"{self.current_file.name} — {coord.upper()} Axis")
+        self.ax.set_title(f"{self.current_file.name} — {c.upper()} Axis")
-        self.ax.legend(loc="upper right")
+        self.ax.legend(loc="upper right", fontsize="small")
-        self.ax.grid(True)
+        self.ax.grid(True, alpha=0.3)
 
-        ymin, ymax = self.ax.get_ylim()
-        self.ax.set_ylim(min(ymin, -1.1), max(ymax, 1.1))
         self.canvas.draw()
 
 

configurations/pid_cont/rate_01.toml

@@ -1,16 +0,0 @@
-
-layers = [
-    { type = "Rate", max_rate = 3.14, kp = [
-        0.01,
-        0.1,
-        0.01,
-    ], ki = [
-        0.0,
-        0.0,
-        0.0,
-    ], kd = [
-        0.0,
-        0.0,
-        0.0,
-    ], frequency = 600.0 },
-]

configurations/pid_cont/rate_02.toml

@@ -1,16 +1,26 @@
 
-layers = [
+stack = { max_rate = 3.14, rotation_pid = { kp = [
-    { type = "Rate", max_rate = 3.14, kp = [
+    0.5,
-        0.02,
+    0.5,
-        0.2,
+    0.5,
-        0.02,
+], ki = [
-    ], ki = [
     0.0,
     0.0,
     0.0,
-    ], kd = [
+], kd = [
     0.0,
     0.0,
     0.0,
-    ], frequency = 600.0 },
+], frequency = 100.0 }, rate_pid = { kp = [
-]
+    0.03,
+    0.3,
+    0.03,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 600.0 } }

configurations/pid_cont/rate_025.toml

@@ -0,0 +1,26 @@
+
+stack = { max_rate = 3.14, rotation_pid = { kp = [
+    0.7,
+    0.7,
+    0.7,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 50.0 }, rate_pid = { kp = [
+    0.03,
+    0.3,
+    0.03,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 600.0 } }

configurations/pid_cont/rate_03.toml

@@ -1,16 +1,26 @@
 
-layers = [
+stack = { max_rate = 3.14, rotation_pid = { kp = [
-    { type = "Rate", max_rate = 3.14, kp = [
+    1.0,
-        0.02,
+    1.0,
-        0.2,
+    1.0,
-        0.02,
+], ki = [
-    ], ki = [
     0.0,
     0.0,
     0.0,
-    ], kd = [
+], kd = [
     0.0,
     0.0,
     0.0,
-    ], frequency = 600.0 },
+], frequency = 50.0 }, rate_pid = { kp = [
-]
+    0.03,
+    0.3,
+    0.03,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 600.0 } }

inputs/jsontest.json

@@ -0,0 +1 @@
+not/jsontest.json

inputs/rot_all.json

@@ -0,0 +1,92 @@
+{
+  "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
+        },
+        "mode": "Rotation"
+      },
+      "time": 0
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": 3.14,
+          "yaw": 3.1,
+          "pitch": 1.5
+        },
+        "mode": "Rotation"
+      },
+      "time": 3
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": -1.5,
+          "yaw": 0.8,
+          "pitch": 0.4
+        },
+        "mode": "Rotation"
+      },
+      "time": 10
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": 1.5,
+          "yaw": 0.1,
+          "pitch": 2
+        },
+        "mode": "Rotation"
+      },
+      "time": 20
+    }
+  ]
+}

inputs/rot_x.json

@@ -0,0 +1,92 @@
+{
+  "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
+        },
+        "mode": "Rotation"
+      },
+      "time": 0
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": 3.14,
+          "yaw": 0,
+          "pitch": 0
+        },
+        "mode": "Rotation"
+      },
+      "time": 3
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": -1.5,
+          "yaw": 0,
+          "pitch": 0
+        },
+        "mode": "Rotation"
+      },
+      "time": 10
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": 1.5,
+          "yaw": 0,
+          "pitch": 0
+        },
+        "mode": "Rotation"
+      },
+      "time": 20
+    }
+  ]
+}

inputs/rot_y.json

@@ -0,0 +1,92 @@
+{
+  "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
+        },
+        "mode": "Rotation"
+      },
+      "time": 0
+    },
+    {
+      "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": 3.14,
+          "pitch": 0
+        },
+        "mode": "Rotation"
+      },
+      "time": 3
+    },
+    {
+      "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": -1.5,
+          "pitch": 0
+        },
+        "mode": "Rotation"
+      },
+      "time": 10
+    },
+    {
+      "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": 1.5,
+          "pitch": 0
+        },
+        "mode": "Rotation"
+      },
+      "time": 20
+    }
+  ]
+}

inputs/rot_z.json

@@ -0,0 +1,92 @@
+{
+  "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
+        },
+        "mode": "Rotation"
+      },
+      "time": 0
+    },
+    {
+      "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": 3.14
+        },
+        "mode": "Rotation"
+      },
+      "time": 3
+    },
+    {
+      "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": -1.5
+        },
+        "mode": "Rotation"
+      },
+      "time": 10
+    },
+    {
+      "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": 1.5
+        },
+        "mode": "Rotation"
+      },
+      "time": 20
+    }
+  ]
+}

src/config.rs

@@ -1,3 +1,4 @@
+use nalgebra::{vector, Vector3};
 use rapier3d::prelude::*;
 use serde::Deserialize;
 
@@ -6,25 +7,31 @@ pub struct PidConfig {
     pub kp: [f32; 3],
     pub ki: [f32; 3],
     pub kd: [f32; 3],
+    pub frequency: f32,
 }
 
+impl PidConfig {
+    pub fn p_vec(&self) -> Vector3<f32> {
+        vector![self.kp[0], self.kp[1], self.kp[2]]
+    }
+    pub fn i_vec(&self) -> Vector3<f32> {
+        vector![self.ki[0], self.ki[1], self.ki[2]]
+    }
+    pub fn d_vec(&self) -> Vector3<f32> {
+        vector![self.kd[0], self.kd[1], self.kd[2]]
+    }
+}
+
+/// Now each layer is explicitly typed
 #[derive(Debug, Deserialize, Clone)]
-#[serde(tag = "type")]
+pub struct ControllerStackConfig {
-pub enum LayerConfig {
+    /// PID for the rotation (angle → angular rate) layer
-    /// Controls angular velocity (Input: joystick/previous layer -> Output: torque)
+    pub rotation_pid: PidConfig,
-    Rate {
+
-        #[serde(flatten)]
+    /// PID for the angular rate (angular rate → torque) layer
-        pid: PidConfig,
+    pub rate_pid: PidConfig,
-        max_rate: f32,
+    /// Maximum angular rate (rad/s) that joystick input maps to
-        frequency: f32,
+    pub max_rate: f32,
-    },
-    /// Controls orientation (Input: joystick -> Output: desired angular velocity)
-    Angle {
-        #[serde(flatten)]
-        pid: PidConfig,
-        max_angle: f32,
-        frequency: f32,
-    },
 }
 
 #[derive(Debug, Deserialize, Clone)]
@@ -32,9 +39,8 @@ pub struct SimulationConfig {
     pub tickrate: f32,
     pub drone_tick_rate: u64,
 
-    // --- Modular Controller Stack ---
+    /// Controller stack
-    // The order of this Vec defines the stack (e.g., [Angle, Rate])
+    pub stack: ControllerStackConfig,
-    pub layers: Vec<LayerConfig>,
 
     /// Maps [Roll, Yaw, Pitch] to each of the 4 motors
     pub motor_map: [[f32; 3]; 4],
@@ -47,15 +53,3 @@ pub struct SimulationConfig {
     pub time_constant: f32,
     pub mass: f32,
 }
-
-impl PidConfig {
-    pub fn p_vec(&self) -> Vector<f32> {
-        vector![self.kp[0], self.kp[1], self.kp[2]]
-    }
-    pub fn i_vec(&self) -> Vector<f32> {
-        vector![self.ki[0], self.ki[1], self.ki[2]]
-    }
-    pub fn d_vec(&self) -> Vector<f32> {
-        vector![self.kd[0], self.kd[1], self.kd[2]]
-    }
-}

src/drone.rs

@@ -53,6 +53,7 @@ pub struct Drone {
     width: f32,
     height: f32,
     pub current_throttles: [f32; 4],
+    target_throttles: [f32; 4],
     last_time: f32,
 }
 
@@ -107,6 +108,7 @@ impl Drone {
             width,
             height,
             current_throttles: [0.0; 4],
+            target_throttles: [0.0; 4],
             last_time: world.get_time(),
         };
     }
@@ -127,7 +129,7 @@ impl Drone {
     }
 
     fn apply_throttles(&mut self, world: &mut World, dt: f32) {
-        let throttles = self.controller.get_motor_throttles();
+        let throttles = self.target_throttles;
 
         let drone_rb = world.bodies.get_mut(self.rb_handle).unwrap();
 
@@ -186,6 +188,7 @@ impl Drone {
         self.controller
             .set_angular_velocity(rb.rotation().inverse().transform_vector(&rb.angvel()));
         self.controller.set_rotation(*rb.rotation());
+        self.target_throttles = self.controller.get_motor_throttles();
     }
 
     pub fn get_angvel(&self, world: &World) -> na::Vector3<f32> {

src/drone/input.rs

@@ -30,6 +30,7 @@ pub struct RotationInput {
 pub enum ModeInput {
     #[default]
     Acro,
+    Rotation,
     Navigation,
 }
 

src/drone/stacked.rs

@@ -1,10 +1,10 @@
 #![allow(dead_code)]
 
-use nalgebra::{self as na, vector};
+use nalgebra as na;
 use std::{any::Any, f32};
 
+use crate::drone::input::ModeInput;
 use crate::drone::stacked::modules::ModuleRuntime;
-use crate::drone::JoystickInput;
 use crate::drone::{controller::DroneController, input::Input};
 
 use crate::config::*;
@@ -13,7 +13,7 @@ pub mod mixer;
 pub mod modules;
 
 use mixer::MotorMixer;
-use modules::{ControllerModule, PidProcessor};
+use modules::*;
 
 pub struct DroneState {
     pub rotation: na::UnitQuaternion<f32>,
@@ -21,9 +21,11 @@ pub struct DroneState {
 }
 
 pub struct StackedController {
-    modules: Vec<ModuleRuntime>,
+    pub rotation_rt: ModuleRuntime<RotationController>,
-    config: SimulationConfig,
+    pub rate_rt: ModuleRuntime<AngularRateController>,
+
     mixer: MotorMixer,
+    config: SimulationConfig,
 
     // State
     drone_state: DroneState,
@@ -33,54 +35,19 @@ pub struct StackedController {
 }
 
 impl StackedController {
-    pub fn set_input(&mut self, inp: Input) {
-        self.input = inp;
-    }
     pub fn new(config: SimulationConfig) -> Self {
-        let mut modules = Vec::new();
+        let rotation_ctrl = RotationController::new(PidProcessor::new(&config.stack.rotation_pid));
-
+        let rate_ctrl = AngularRateController::new(PidProcessor::new(&config.stack.rate_pid));
-        for layer in &config.layers {
-            let (module, freq) = match layer {
-                LayerConfig::Angle {
-                    pid,
-                    max_angle,
-                    frequency,
-                } => (
-                    ControllerModule::Rotation {
-                        processor: PidProcessor::new(pid),
-                        max_angle: *max_angle,
-                    },
-                    *frequency,
-                ),
-                LayerConfig::Rate {
-                    pid,
-                    max_rate,
-                    frequency,
-                } => (
-                    ControllerModule::AngularRate {
-                        processor: PidProcessor::new(pid),
-                        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 {
+            rotation_rt: ModuleRuntime::new(rotation_ctrl, config.stack.rotation_pid.frequency),
+            rate_rt: ModuleRuntime::new(rate_ctrl, config.stack.rate_pid.frequency),
             mixer: MotorMixer {
                 motor_map: config.motor_map,
                 min_throttle: 0.1,
                 max_throttle: 1.0,
                 mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 },
             },
-            modules, // Now Vec<ModuleRuntime>
             config,
             input: Input::default(),
             drone_state: DroneState {
@@ -91,6 +58,10 @@ impl StackedController {
             current_time: 0.0,
         }
     }
+
+    pub fn set_input(&mut self, inp: Input) {
+        self.input = inp;
+    }
 }
 
 impl DroneController for StackedController {
@@ -108,37 +79,34 @@ impl DroneController for StackedController {
     fn get_motor_throttles(&mut self) -> [f32; 4] {
         let frame_dt = (self.current_time - self.last_time).max(0.0);
 
-        // Initial setpoint comes from the sticks
+        let angular_rate_setpoint = if self.input.mode != ModeInput::Acro {
-        let mut current_setpoint = vector![
+            let rotation_setpoint = if self.input.mode != ModeInput::Rotation {
+                panic!("Not Implemented")
+            } else {
+                // println!("Rotation!");
+                Rotation(na::vector![
+                    self.input.rotation.roll,
+                    self.input.rotation.yaw,
+                    self.input.rotation.pitch,
+                ])
+            };
+            self.rotation_rt
+                .update(rotation_setpoint, &self.drone_state, frame_dt)
+        } else {
+            AngularRate(
+                na::vector![
                     self.input.joystick.roll_input,
                     self.input.joystick.yaw_input,
                     self.input.joystick.pitch_input,
-        ];
+                ] * self.config.stack.max_rate,
+            )
+        };
 
-        for (i, runtime) in self.modules.iter_mut().enumerate() {
+        let torque = self
-            runtime.accumulated_time += frame_dt;
+            .rate_rt
+            .update(angular_rate_setpoint, &self.drone_state, frame_dt);
 
-            if runtime.accumulated_time >= runtime.target_dt {
+        self.mixer.mix(self.input.joystick.throttle_input, torque.0)
-                // 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;
-        }
-
-        self.mixer
-            .mix(self.input.joystick.throttle_input, current_setpoint)
     }
 
     fn as_any(&self) -> &dyn Any {

src/drone/stacked/modules.rs

@@ -2,70 +2,74 @@ use crate::config::PidConfig;
 use crate::drone::stacked::DroneState;
 use nalgebra as na;
 
-pub enum ControllerModule {
+pub mod rate;
-    AngularRate {
+pub mod rot;
-        processor: PidProcessor,
+
-        max_rate: f32,
+pub use rate::AngularRateController;
-    },
+pub use rot::RotationController;
-    Rotation {
+
-        processor: PidProcessor,
+pub trait ControllerModule {
-        max_angle: f32,
+    type Input: Clone + Default;
-    },
+    type Output: Clone + Default;
+
+    fn process(&mut self, input: Self::Input, state: &DroneState, dt: f32) -> Self::Output;
 }
 
-pub struct ModuleRuntime {
+pub struct ModuleRuntime<C>
-    pub module: ControllerModule,
+where
-    pub target_dt: f32, // e.g., 0.01 for 100Hz
+    C: ControllerModule,
+{
+    pub module: C,
+    pub target_dt: f32,
     pub accumulated_time: f32,
-    pub last_output: na::Vector3<f32>, // Store the last result to pass down the chain
+    pub last_output: Option<C::Output>,
 }
 
-impl ControllerModule {
+impl<C> ModuleRuntime<C>
-    pub fn process(
+where
-        &mut self,
+    C: ControllerModule,
-        setpoint: na::Vector3<f32>,
+{
-        state: &DroneState,
+    pub fn update(&mut self, input: C::Input, state: &DroneState, dt: f32) -> C::Output {
-        dt: f32,
+        self.accumulated_time += dt;
-        is_first_layer: bool,
+
-    ) -> na::Vector3<f32> {
+        if self.accumulated_time >= self.target_dt {
-        match self {
+            self.accumulated_time = 0.0;
-            ControllerModule::Rotation {
+
-                processor,
+            let output = self.module.process(input, state, self.target_dt);
-                max_angle,
+            self.last_output = Some(output.clone());
-            } => {
+            output
-                // Setpoint is -1.0..1.0, scale it to target Radians
-                let target_angles = if is_first_layer {
-                    setpoint * *max_angle
         } else {
-                    setpoint
+            self.last_output
-                };
+                .as_ref()
-
+                .unwrap_or(&C::Output::default())
-                let (r, p, y) = state.rotation.euler_angles();
+                .clone()
-                let current_angles = na::vector![r, y, p];
-
-                // Output of Angle PID = Desired Angular Velocity
-                processor.update(target_angles, current_angles, dt)
         }
-
-            ControllerModule::AngularRate {
-                processor,
-                max_rate,
-            } => {
-                // If Rate is the start of the chain (Acro mode), scale the joystick.
-                // If it's the second layer, the setpoint is already a velocity from the Angle layer.
-                let target_velocity = if is_first_layer {
-                    setpoint * *max_rate
-                } else {
-                    setpoint
-                };
-
-                // Output of Rate PID = Desired Torque/Correction Force
-                processor.update(target_velocity, state.angular_velocity, dt)
     }
+    pub fn new(module: C, frequency: f32) -> Self {
+        Self {
+            module,
+            target_dt: 1.0 / frequency,
+            accumulated_time: 0.0,
+            last_output: None,
         }
     }
 }
 
+#[derive(Clone, Copy, Debug, Default)]
+pub struct Position(pub na::Vector3<f32>); // meters
+
+#[derive(Clone, Copy, Debug, Default)]
+pub struct Velocity(pub na::Vector3<f32>); // m/s
+
+#[derive(Clone, Copy, Debug, Default)]
+pub struct Rotation(pub na::Vector3<f32>); // radians
+
+#[derive(Clone, Copy, Debug, Default)]
+pub struct AngularRate(pub na::Vector3<f32>); // rad/s
+
+#[derive(Clone, Copy, Debug, Default)]
+pub struct Torque(pub na::Vector3<f32>); // control output
+
 pub struct PidProcessor {
     kp: na::Vector3<f32>,
     ki: na::Vector3<f32>,

src/drone/stacked/modules/rate.rs

@@ -0,0 +1,25 @@
+use crate::drone::stacked::modules::*;
+
+pub struct AngularRateController {
+    pid: PidProcessor,
+}
+
+impl AngularRateController {
+    pub fn new(pid: PidProcessor) -> Self {
+        Self { pid }
+    }
+}
+
+impl ControllerModule for AngularRateController {
+    type Input = AngularRate;
+    type Output = Torque;
+
+    fn process(&mut self, input: AngularRate, state: &DroneState, dt: f32) -> Torque {
+        // Scale normalized rate command
+        let target_rate = input.0;
+
+        let output = self.pid.update(target_rate, state.angular_velocity, dt);
+
+        Torque(output)
+    }
+}

src/drone/stacked/modules/rot.rs

@@ -0,0 +1,29 @@
+use nalgebra::{Quaternion, UnitQuaternion};
+
+use crate::drone::stacked::modules::*;
+
+pub struct RotationController {
+    pid: PidProcessor,
+}
+impl RotationController {
+    pub fn new(pid: PidProcessor) -> Self {
+        Self { pid }
+    }
+}
+
+impl ControllerModule for RotationController {
+    type Input = Rotation;
+    type Output = AngularRate;
+
+    fn process(&mut self, input: Rotation, state: &DroneState, dt: f32) -> AngularRate {
+        let target_rot = UnitQuaternion::from_scaled_axis(input.0);
+
+        let error_quat = state.rotation.rotation_to(&target_rot);
+
+        let error_vector = error_quat.scaled_axis();
+
+        let output = self.pid.update(error_vector, na::Vector3::zeros(), dt);
+
+        AngularRate(state.rotation.inverse_transform_vector(&output))
+    }
+}

src/logger.rs

@@ -1,20 +1,22 @@
 use nalgebra as na;
 use serde::Serialize;
+use std::io::Write;
 
-pub struct CsvLogger {
+pub struct Logger {
-    writer: csv::Writer<std::fs::File>,
+    writer: std::io::BufWriter<std::fs::File>,
 }
 
-impl CsvLogger {
+impl Logger {
     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),
+            writer: std::io::BufWriter::new(file),
         })
     }
 
     pub fn log(&mut self, row: &SimLogRow) -> Result<(), Box<dyn std::error::Error>> {
-        self.writer.serialize(row)?;
+        serde_json::to_writer(&mut self.writer, row)?;
+        self.writer.write_all(b"\n")?; // newline-delimited JSON
         Ok(())
     }
 
@@ -24,51 +26,35 @@ impl CsvLogger {
     }
 }
 
+#[derive(Debug, Serialize)]
+pub struct Vec3Serialize {
+    pub x: f32,
+    pub y: f32,
+    pub z: f32,
+}
+
+impl From<na::Vector3<f32>> for Vec3Serialize {
+    fn from(value: na::Vector3<f32>) -> Self {
+        Self {
+            x: value.x,
+            y: value.y,
+            z: value.z,
+        }
+    }
+}
+impl From<Vec3Serialize> for na::Vector3<f32> {
+    fn from(value: Vec3Serialize) -> Self {
+        na::vector![value.x, value.y, value.z]
+    }
+}
+
 #[derive(Debug, Serialize)]
 pub struct SimLogRow {
     pub time: f32,
-    pub target_x: f32,
+    pub angvel_target: Vec3Serialize,
-    pub target_y: f32,
+    pub angvel_current: Vec3Serialize,
-    pub target_z: f32,
+    pub mot_current: Vec3Serialize,
-
+    pub mot_target: Vec3Serialize,
-    pub current_x: f32,
+    pub rot_target: Vec3Serialize,
-    pub current_y: f32,
+    pub rot_current: Vec3Serialize,
-    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,
-        }
-    }
 }

src/main.rs

@@ -26,6 +26,10 @@ use std::thread::JoinHandle;
 
 fn main() {
     run_batch();
+    // run(
+    //     &"./inputs/rot.json".into(),
+    //     &"./configurations/final/sim_std_mot_std_rate_03.toml".into(),
+    // );
 }
 
 fn run_batch() {
@@ -82,7 +86,7 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
         config.mass,
     );
 
-    let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name);
+    let result_file = format!("{}/{}_{}.json", RESULTS_DIR, input_name, config_name);
 
     let mut sim = Simulation::new(
         drone,

src/main_testing.rs

@@ -0,0 +1,16 @@
+use nalgebra as na;
+
+fn main() {
+    let rot = na::UnitQuaternion::from_scaled_axis(na::vector![3.14, 1.5, 0.0]);
+
+    let target = na::UnitQuaternion::from_scaled_axis(na::vector![-1.0, 0.0, 0.0]);
+
+    let error_quat = rot.rotation_to(&target);
+
+    let error_vector = error_quat.scaled_axis();
+
+    println!("start: {:}", rot.scaled_axis());
+    println!("target: {:}", target.scaled_axis());
+    println!("error: {:}", error_vector);
+    println!("final: {:}", (error_quat * rot).scaled_axis());
+}

src/simulation.rs

@@ -1,5 +1,5 @@
 use macroquad::prelude as mq;
-use nalgebra as na;
+use nalgebra::{self as na, vector};
 use rapier3d::prelude as rp;
 use std::error::Error;
 
@@ -10,7 +10,7 @@ use crate::{
         Drone,
     },
     engine::World,
-    logger::CsvLogger,
+    logger::{Logger, SimLogRow},
     rendering::Renderer,
 };
 
@@ -37,7 +37,7 @@ pub struct Simulation {
     pub world: World,
     pub mode: SimMode,
 
-    logger: Option<CsvLogger>,
+    logger: Option<Logger>,
     drone_tick_rate: u64,
 }
 
@@ -50,7 +50,7 @@ impl Simulation {
         drone_tick_rate: u64,
     ) -> Self {
         let logger = match &results_file {
-            Some(path) => Some(CsvLogger::new(path).unwrap()),
+            Some(path) => Some(Logger::new(path).unwrap()),
             None => None,
         };
 
@@ -159,12 +159,22 @@ impl Simulation {
         }
         self.drone.process_tick(&mut self.world);
 
-        let target_angular_vel: na::Vector3<f32> = na::vector![
+        let mut target_angular_vel: na::Vector3<f32> = na::vector![
             current_input.joystick.roll_input,
             current_input.joystick.yaw_input,
             current_input.joystick.pitch_input,
         ] * 3.14;
 
+        let mut target_mot: na::Vector3<f32> = Default::default();
+        if let Some(cont) = self
+            .drone
+            .controller
+            .as_mut_any()
+            .downcast_mut::<crate::drone::stacked::StackedController>()
+        {
+            target_angular_vel = cont.rotation_rt.last_output.unwrap_or_default().0;
+            target_mot = cont.rate_rt.last_output.unwrap_or_default().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]
@@ -179,17 +189,26 @@ impl Simulation {
 
         if let Some(logger) = &mut self.logger {
             logger
-                .log(&crate::logger::SimLogRow::from_data(
+                .log(&SimLogRow {
-                    self.world.get_time(),
+                    time: self.world.get_time(),
-                    target_angular_vel,
+                    angvel_target: target_angular_vel.into(),
-                    self.drone
+                    angvel_current: self
+                        .drone
                         .get_rot(&self.world)
                         .inverse()
                         .transform_vector(&self.drone.get_angvel(&self.world))
                         .into(),
-                    applied_motor_diff,
+                    rot_current: self.drone.get_rot(&self.world).scaled_axis().into(),
-                    na::vector![0.0, 0.0, 0.0],
+                    rot_target: na::UnitQuaternion::from_scaled_axis(vector![
-                ))
+                        current_input.rotation.roll,
+                        current_input.rotation.yaw,
+                        current_input.rotation.pitch
+                    ])
+                    .scaled_axis()
+                    .into(),
+                    mot_current: applied_motor_diff.into(),
+                    mot_target: target_mot.into(),
+                })
                 .unwrap();
         }
         Ok(StepOutcome::Continue)