modules almosttt working

configurations/all.toml

@@ -0,0 +1,47 @@
+
+
+
+tickrate = 6000.0
+drone_tick_rate = 600
+max_thrust = 2.6
+max_torque = 0.5
+mass = 0.350
+
+# The Stack: First layer computes target speed, second layer computes motor torque
+layers = [
+    # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
+    { 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,
+    ] },
+]
+
+#                + (map[0] * setpoint.x) // Roll
+#                + (map[1] * setpoint.y) // Yaw
+#                + (map[2] * setpoint.z); // Pitch
+
+# /*
+#  * Motor position indices
+#  *    ^ - Front
+#  *    |
+#  *    |
+#  * 1 --- 0
+#  * |     |
+#  * |     |
+#  * 2 --- 3
+#  */
+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/pitch.toml

@@ -0,0 +1,47 @@
+
+
+
+tickrate = 6000.0
+drone_tick_rate = 600
+max_thrust = 2.6
+max_torque = 0.5
+mass = 0.350
+
+# The Stack: First layer computes target speed, second layer computes motor torque
+layers = [
+    # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
+    { type = "Rate", max_rate = 3.14, kp = [
+        0.0,
+        0.0,
+        0.01,
+    ], ki = [
+        0.0,
+        0.0,
+        0.0,
+    ], kd = [
+        0.0,
+        0.0,
+        0.0,
+    ] },
+]
+
+#                + (map[0] * setpoint.x) // Roll
+#                + (map[1] * setpoint.y) // Yaw
+#                + (map[2] * setpoint.z); // Pitch
+
+# /*
+#  * Motor position indices
+#  *    ^ - Front
+#  *    |
+#  *    |
+#  * 1 --- 0
+#  * |     |
+#  * |     |
+#  * 2 --- 3
+#  */
+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/roll.toml

@@ -0,0 +1,47 @@
+
+
+
+tickrate = 6000.0
+drone_tick_rate = 600
+max_thrust = 2.6
+max_torque = 0.5
+mass = 0.350
+
+# The Stack: First layer computes target speed, second layer computes motor torque
+layers = [
+    # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
+    { type = "Rate", max_rate = 3.14, kp = [
+        0.01,
+        0.0,
+        0.0,
+    ], ki = [
+        0.0,
+        0.0,
+        0.0,
+    ], kd = [
+        0.0,
+        0.0,
+        0.0,
+    ] },
+]
+
+#                + (map[0] * setpoint.x) // Roll
+#                + (map[1] * setpoint.y) // Yaw
+#                + (map[2] * setpoint.z); // Pitch
+
+# /*
+#  * Motor position indices
+#  *    ^ - Front
+#  *    |
+#  *    |
+#  * 1 --- 0
+#  * |     |
+#  * |     |
+#  * 2 --- 3
+#  */
+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/sim_std_mot_std_low_01.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.005, 0.005, 0.005]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_02.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.01, 0.01, 0.01]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_03.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.015, 0.015, 0.015]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_04.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.02, 0.02, 0.02]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_05.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.025, 0.025, 0.025]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_06.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.03, 0.03, 0.03]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_07.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.035, 0.035, 0.035]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_08.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.04, 0.04, 0.04]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_09.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.045, 0.045, 0.045]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_std_low_10.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.05, 0.05, 0.05]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_01.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.005, 0.005, 0.005]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_02.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.01, 0.01, 0.01]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_03.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.015, 0.015, 0.015]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_04.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.02, 0.02, 0.02]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_05.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.025, 0.025, 0.025]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_06.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.03, 0.03, 0.03]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_07.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.035, 0.035, 0.035]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_08.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.04, 0.04, 0.04]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_09.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.045, 0.045, 0.045]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/sim_std_mot_tc_low_10.toml

@@ -1,14 +0,0 @@
-tickrate = 6000.0
-drone_tick_rate = 600
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant=0.01
-
-
-target_rate = 3.141592
-proportional_multiplier = [0.05, 0.05, 0.05]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]

configurations/yaw.toml

@@ -0,0 +1,47 @@
+
+
+
+tickrate = 6000.0
+drone_tick_rate = 600
+max_thrust = 2.6
+max_torque = 2.0
+mass = 0.350
+
+# The Stack: First layer computes target speed, second layer computes motor torque
+layers = [
+    # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
+    { type = "Rate", max_rate = 3.14, kp = [
+        0.0,
+        0.01,
+        0.0,
+    ], ki = [
+        0.0,
+        0.0,
+        0.0,
+    ], kd = [
+        0.0,
+        0.0,
+        0.0,
+    ] },
+]
+
+#                + (map[0] * setpoint.x) // Roll
+#                + (map[1] * setpoint.y) // Yaw
+#                + (map[2] * setpoint.z); // Pitch
+
+# /*
+#  * Motor position indices
+#  *    ^ - Front
+#  *    |
+#  *    |
+#  * 1 --- 0
+#  * |     |
+#  * |     |
+#  * 2 --- 3
+#  */
+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
+]

src/config.rs

@@ -1,49 +1,59 @@
 use rapier3d::prelude::*;
+use serde::Deserialize;
 
-#[derive(Debug, serde::Deserialize)]
+#[derive(Debug, Deserialize, Clone)]
+pub struct PidConfig {
+    pub kp: [f32; 3],
+    pub ki: [f32; 3],
+    pub kd: [f32; 3],
+}
+
+#[derive(Debug, Deserialize, Clone)]
+#[serde(tag = "type")]
+pub enum LayerConfig {
+    /// Controls angular velocity (Input: joystick/previous layer -> Output: torque)
+    Rate {
+        #[serde(flatten)]
+        pid: PidConfig,
+        max_rate: f32,
+    },
+    /// Controls orientation (Input: joystick -> Output: desired angular velocity)
+    Angle {
+        #[serde(flatten)]
+        pid: PidConfig,
+        max_angle: f32,
+    },
+}
+
+#[derive(Debug, Deserialize, Clone)]
 pub struct SimulationConfig {
     pub tickrate: f32,
     pub drone_tick_rate: u64,
 
-    // PID
+    // --- Modular Controller Stack ---
-    pub target_rate: f32,
+    // The order of this Vec defines the stack (e.g., [Angle, Rate])
-    pub proportional_multiplier: [f32; 3],
+    pub layers: Vec<LayerConfig>,
-    pub integral_multiplier: [f32; 3],
-    pub diferential_multiplier: [f32; 3],
 
-    // Motors
+    /// Maps [Pitch, Yaw, Roll] 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,
-
-    // Drone
     pub mass: f32,
 }
 
-impl SimulationConfig {
+impl PidConfig {
-    pub fn proportional(&self) -> Vector<f32> {
+    pub fn p_vec(&self) -> Vector<f32> {
-        vector![
+        vector![self.kp[0], self.kp[1], self.kp[2]]
-            self.proportional_multiplier[0],
-            self.proportional_multiplier[1],
-            self.proportional_multiplier[2]
-        ]
     }
-
+    pub fn i_vec(&self) -> Vector<f32> {
-    pub fn integral(&self) -> Vector<f32> {
+        vector![self.ki[0], self.ki[1], self.ki[2]]
-        vector![
-            self.integral_multiplier[0],
-            self.integral_multiplier[1],
-            self.integral_multiplier[2]
-        ]
     }
-
+    pub fn d_vec(&self) -> Vector<f32> {
-    pub fn diferential(&self) -> Vector<f32> {
+        vector![self.kd[0], self.kd[1], self.kd[2]]
-        vector![
-            self.diferential_multiplier[0],
-            self.diferential_multiplier[1],
-            self.diferential_multiplier[2]
-        ]
     }
 }

src/drone.rs

@@ -7,6 +7,7 @@ pub mod controller;
 pub mod fpvcontroller;
 pub mod input;
 pub mod pidcontroller;
+pub mod stacked;
 use controller::*;
 pub use input::JoystickInput;
 
@@ -36,9 +37,9 @@ impl Default for MotorCharacteristics {
              */
             relative_motor_positions: [
                 rp::point![5.0, 0.0, 5.0],
-                rp::point![-5.0, 0.0, 5.0],
-                rp::point![-5.0, 0.0, -5.0],
                 rp::point![5.0, 0.0, -5.0],
+                rp::point![-5.0, 0.0, -5.0],
+                rp::point![-5.0, 0.0, 5.0],
             ],
             max_thrust: 2.6,
             max_torque: 0.5,
@@ -80,7 +81,8 @@ impl Drone {
                  * A Poor Man's fluid simulation :D
                  */
                 // .linear_damping(0.2) // Damps velocity slowly
-                .angular_damping(0.2) // Damps angular velocity slowly
+                .angular_damping(0.1) // Damps angular velocity slowly
+                .gyroscopic_forces_enabled(true)
                 .build(),
         );
         let width = 0.40;

src/drone/pidcontroller.rs

@@ -2,9 +2,9 @@
 use nalgebra::{self as na, vector};
 use std::{any::Any, f32};
 
-use crate::drone::controller::DroneController;
 use crate::drone::JoystickInput;
 use crate::drone::MotorCharacteristics;
+use crate::drone::controller::DroneController;
 
 #[derive(Default)]
 pub struct PIDControllerState {
@@ -64,7 +64,7 @@ impl PIDController {
         return coords;
     }
 
-    fn get_desired_and_error(&mut self) -> [na::Vector3<f32>; 2] {
+    pub fn get_desired_and_error(&mut self) -> [na::Vector3<f32>; 2] {
         let rot = self.state.current_rotation;
         let current = rot.inverse().transform_vector(&self.get_angular_velocity());
         let target = self.get_desired_angular_velocity();

src/drone/stacked.rs

@@ -0,0 +1,221 @@
+#![allow(dead_code)]
+
+use nalgebra::{self as na, vector};
+use std::{any::Any, f32};
+
+use crate::drone::controller::DroneController;
+use crate::drone::JoystickInput;
+
+use crate::config::*;
+
+pub struct DroneState {
+    pub rotation: na::UnitQuaternion<f32>,
+    pub angular_velocity: na::Vector3<f32>,
+}
+
+pub enum ControllerModule {
+    Rate {
+        processor: PidProcessor,
+        max_rate: f32,
+    },
+    Angle {
+        processor: PidProcessor,
+        max_angle: f32,
+    },
+}
+
+impl ControllerModule {
+    /// Takes the current setpoint (from joystick or previous layer) and
+    /// returns the command for the next layer.
+    pub fn process(
+        &mut self,
+        setpoint: na::Vector3<f32>,
+        state: &DroneState,
+        dt: f32,
+        is_first_layer: bool,
+    ) -> na::Vector3<f32> {
+        match self {
+            ControllerModule::Angle {
+                processor,
+                max_angle,
+            } => {
+                // Setpoint is -1.0..1.0, scale it to target Radians
+                let target_angles = setpoint * *max_angle;
+
+                let (r, p, y) = state.rotation.euler_angles();
+                let current_angles = na::vector![r, y, p];
+
+                // Output of Angle PID = Desired Angular Velocity
+                processor.update(target_angles, current_angles, dt)
+            }
+
+            ControllerModule::Rate {
+                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
+                };
+                let rot = state.rotation;
+                let current = rot.inverse().transform_vector(&state.angular_velocity);
+
+                // Output of Rate PID = Desired Torque/Correction Force
+                processor.update(target_velocity, current, dt)
+            }
+        }
+    }
+}
+
+pub struct StackedController {
+    modules: Vec<ControllerModule>,
+    config: SimulationConfig,
+
+    // State
+    drone_state: DroneState,
+    input: JoystickInput,
+    last_time: f32,
+    current_time: f32,
+}
+
+impl StackedController {
+    pub fn set_input(&mut self, inp: JoystickInput) {
+        self.input = inp;
+    }
+    pub fn new(config: SimulationConfig) -> Self {
+        let mut modules = Vec::new();
+
+        for layer in &config.layers {
+            match layer {
+                LayerConfig::Angle { pid, max_angle } => {
+                    modules.push(ControllerModule::Angle {
+                        processor: PidProcessor::new(pid),
+                        max_angle: *max_angle,
+                    });
+                }
+                LayerConfig::Rate { pid, max_rate } => {
+                    modules.push(ControllerModule::Rate {
+                        processor: PidProcessor::new(pid),
+                        max_rate: *max_rate,
+                    });
+                }
+            }
+        }
+
+        Self {
+            modules,
+            config,
+            input: JoystickInput::default(),
+            drone_state: DroneState {
+                rotation: na::UnitQuaternion::identity(),
+                angular_velocity: na::Vector3::zeros(),
+            },
+            last_time: 0.0,
+            current_time: 0.0,
+        }
+    }
+}
+
+impl DroneController for StackedController {
+    fn set_rotation(&mut self, rot: na::UnitQuaternion<f32>) {
+        self.drone_state.rotation = rot;
+    }
+    fn set_angular_velocity(&mut self, vel: na::Vector3<f32>) {
+        self.drone_state.angular_velocity = vel;
+    }
+    fn set_time(&mut self, t: f32) {
+        self.last_time = self.current_time;
+        self.current_time = t;
+    }
+
+    fn get_motor_throttles(&mut self) -> [f32; 4] {
+        let dt = (self.current_time - self.last_time).max(0.0001);
+
+        // Input normalized -1.0 to 1.0 from joystick
+        let mut setpoint = vector![
+            self.input.roll_input,
+            self.input.yaw_input,
+            self.input.pitch_input,
+        ];
+
+        // --- CASCADED PROCESSING ---
+        // If Layer 0 is Angle, setpoint becomes Desired Rate.
+        // If Layer 1 is Rate, it takes that Desired Rate and outputs Torque.
+        //
+
+        for (i, module) in self.modules.iter_mut().enumerate() {
+            let is_first_layer = i == 0;
+            setpoint = module.process(setpoint, &self.drone_state, dt, is_first_layer);
+        }
+
+        // --- MOTOR MIXER ---
+        let mut motors = [0.0; 4];
+        for i in 0..4 {
+            let map = self.config.motor_map[i];
+            motors[i] = self.input.throttle_input
+                + (map[0] * setpoint.x) // Roll
+                + (map[1] * setpoint.y) // Yaw
+                + (map[2] * setpoint.z); // Pitch
+        }
+
+        // Clamp while keeping relative values
+        let max = motors
+            .iter()
+            .copied()
+            .max_by(|a, b| a.total_cmp(b))
+            .unwrap_or(0.0);
+        if max > 1.0 {
+            for v in motors.iter_mut() {
+                *v /= max;
+            }
+        }
+
+        motors
+    }
+
+    fn as_any(&self) -> &dyn Any {
+        self
+    }
+    fn as_mut_any(&mut self) -> &mut dyn Any {
+        self
+    }
+}
+
+pub struct PidProcessor {
+    kp: na::Vector3<f32>,
+    ki: na::Vector3<f32>,
+    kd: na::Vector3<f32>,
+    integral: na::Vector3<f32>,
+    last_error: na::Vector3<f32>,
+}
+
+impl PidProcessor {
+    fn new(c: &PidConfig) -> Self {
+        Self {
+            kp: c.p_vec(),
+            ki: c.i_vec(),
+            kd: c.d_vec(),
+            integral: na::Vector3::zeros(),
+            last_error: na::Vector3::zeros(),
+        }
+    }
+
+    fn update(
+        &mut self,
+        target: na::Vector3<f32>,
+        current: na::Vector3<f32>,
+        dt: f32,
+    ) -> na::Vector3<f32> {
+        let error = target - current;
+        self.integral += error * dt;
+        let derivative = (error - self.last_error) / dt;
+        self.last_error = error;
+
+        error.component_mul(&self.kp)
+            + self.integral.component_mul(&self.ki)
+            + derivative.component_mul(&self.kd)
+    }
+}

src/main.rs

@@ -8,6 +8,7 @@ mod camera;
 mod drone;
 mod rendering;
 
+mod config;
 mod graphics_util;
 
 use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer};

src/main_batch.rs

@@ -1,5 +1,3 @@
-use macroquad::prelude as mq;
-
 mod engine;
 use engine::*;
 mod camera;
@@ -22,48 +20,15 @@ const INPUTS_DIR: &str = "inputs/";
 const CONFIGS_DIR: &str = "configurations/";
 const RESULTS_DIR: &str = "results/";
 
-fn window_conf() -> mq::Conf {
-    mq::Conf {
-        window_title: "RustDroneSim".to_owned(),
-        window_resizable: true,
-        platform: mq::miniquad::conf::Platform {
-            ..Default::default()
-        },
-        ..Default::default()
-    }
-}
-
 use std::env;
 use std::path::PathBuf;
 use std::thread::JoinHandle;
 
-enum RunMode {
+fn main() {
-    Batch,
+    run_batch();
-    Record { output: String },
 }
 
-fn parse_cli() -> RunMode {
+fn run_batch() {
-    let mut args = env::args().skip(1);
-
-    match args.next().as_deref() {
-        Some("record") => {
-            let output = args.next().expect("record mode requires output file path");
-            RunMode::Record { output }
-        }
-        Some("batch") | None => RunMode::Batch,
-        Some(other) => panic!("Unknown mode: {}", other),
-    }
-}
-
-#[macroquad::main(window_conf)]
-async fn main() {
-    match parse_cli() {
-        RunMode::Batch => run_batch().await,
-        RunMode::Record { output } => run_record(output).await,
-    }
-}
-
-async fn run_batch() {
     let _ = fs::remove_dir_all(RESULTS_DIR);
     let _ = fs::create_dir_all(RESULTS_DIR);
 
@@ -107,13 +72,7 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
 
     let drone = drone::Drone::new(
         &mut world,
-        Box::new(drone::pidcontroller::PIDController {
+        Box::new(drone::stacked::StackedController::new(config.clone())),
-            target_rate: config.target_rate,
-            proportional_multiplier: config.proportional(),
-            integral_multiplier: config.integral(),
-            diferential_multiplier: config.diferential(),
-            ..Default::default()
-        }),
         drone::MotorCharacteristics {
             max_thrust: config.max_thrust,
             max_torque: config.max_torque,

src/rendering.rs

@@ -91,8 +91,8 @@ impl MaterialUni {
         }
     }
     pub fn as_type(&self) -> mq::UniformType {
-        use mq::UniformType::*;
         use MaterialUni::*;
+        use mq::UniformType::*;
         match self {
             RenderNormalsBool => Int1,
             RenderShadowsBool => Int1,

src/rendering/ui.rs

@@ -1,6 +1,6 @@
 use crate::rendering::{self, MaterialUni};
 use macroquad::prelude::*;
-use macroquad::ui::{hash, Ui};
+use macroquad::ui::{Ui, hash};
 
 use strum::IntoEnumIterator;
 

src/simulation.rs

@@ -218,7 +218,7 @@ impl Simulation {
                     .drone
                     .controller
                     .as_mut_any()
-                    .downcast_mut::<crate::drone::pidcontroller::PIDController>()
+                    .downcast_mut::<crate::drone::stacked::StackedController>()
                 {
                     cont.set_input(current_input);
                 }
@@ -246,17 +246,21 @@ impl Simulation {
                     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];
+            }
+
             let m = self.drone.current_throttles;
             applied_motor_diff = na::vector![
-                    (m[2] + m[3] - m[0] - m[1]),
+                (m[1] + m[2] - m[0] - m[3]),
                 (m[0] + m[2] - m[1] - m[3]),
-                    (m[0] + m[3] - m[1] - m[2])
+                (m[2] + m[3] - m[0] - m[1])
             ] / 2.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(),