separate repo

Cargo.toml

@@ -0,0 +1,21 @@
+[package]
+name = "drone_controller"
+version = "0.1.0"
+edition = "2024"
+
+[dependencies]
+
+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"] }
+
+[lib]
+crate-type = ["lib", "cdylib"]
+
+[build-dependencies]
+cbindgen = "0.29"

build.rs

@@ -0,0 +1,11 @@
+extern crate cbindgen;
+
+use std::env;
+
+fn main() {
+    let crate_dir = env::var("CARGO_MANIFEST_DIR").unwrap();
+
+    let _ = cbindgen::generate(crate_dir)
+        .unwrap()
+        .write_to_file("bindings.h");
+}

cbindgen.toml

@@ -0,0 +1,36 @@
+# This is a template cbindgen.toml file with all of the default values.
+# Some values are commented out because their absence is the real default.
+#
+# See https://github.com/mozilla/cbindgen/blob/main/docs.md#cbindgentoml
+# for detailed documentation of every option here.
+
+
+language = "C++"
+
+
+include_guard = "my_bindings_h"
+autogen_warning = "/* Warning, this file is autogenerated by cbindgen. Don't modify this manually. */"
+include_version = true
+
+
+braces = "SameLine"
+line_length = 100
+tab_width = 2
+documentation = true
+documentation_style = "auto"
+documentation_length = "full"
+line_endings = "LF"
+
+namespace = "dcont"
+
+
+style = "both"
+sort_by = "Name"
+usize_is_size_t = true
+
+[export]
+include = []
+exclude = []
+# prefix = "DCONT_"
+item_types = []
+renaming_overrides_prefixing = false

src/config.rs

@@ -0,0 +1,59 @@
+use nalgebra::{vector, Vector3};
+use serde::Deserialize;
+
+#[repr(C)]
+#[derive(Debug, Deserialize, Clone)]
+pub struct ControllerStackConfig {
+    /// PID for the rotation (angle → angular rate) layer
+    #[serde(default)]
+    pub rotation_pid: PidConfig,
+
+    #[serde(default)]
+    pub linvel_pid: PidConfig,
+
+    #[serde(default)]
+    pub position_pid: PidConfig,
+
+    /// PID for the angular rate (angular rate → torque) layer
+    pub rate_pid: PidConfig,
+
+    /// Maximum angular rate (rad/s) that joystick input maps to
+    pub max_rate: f32,
+}
+
+#[repr(C)]
+pub struct ControllerConfig {
+    pub stack: ControllerStackConfig,
+
+    // For each motor, [Pitch, Roll, Yaw] (Rotation around x,y,z)
+    pub motor_map: [[f32; 3]; 4],
+
+    pub mass: f32,
+
+    // Per Motor
+    pub max_thrust: f32,
+
+    // Per Motor
+    pub max_torque: f32,
+}
+
+#[repr(C)]
+#[derive(Default, Debug, Deserialize, Clone)]
+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]]
+    }
+}

src/controller.rs

@@ -0,0 +1,94 @@
+use nalgebra as na;
+use std::any::Any;
+
+pub trait DroneController {
+    // Allow downcast of trait -> class.
+    //
+    // This gives us the ability to have a Box<dyn DroneController>
+    // And transform it into a pointer to its class.
+    fn as_any(&self) -> &dyn Any;
+    fn as_mut_any(&mut self) -> &mut dyn Any;
+
+    /*
+     * Methods called by Drone, to transmit information to the controller.
+     */
+    fn set_rotation(&mut self, _rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {}
+    fn set_angular_velocity(&mut self, _angvel: nalgebra::Vector3<f32>) {}
+
+    fn set_linvel(&mut self, _vel: na::Vector3<f32>) {}
+    fn set_pos(&mut self, _pos: na::Vector3<f32>) {}
+    fn set_time(&mut self, _time: f32) {}
+    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
+
+    /*
+     * Throttle should be between 0 and 1. Values will be by the Drone class.
+     */
+    fn get_motor_throttles(&mut self) -> [f32; 4];
+}
+
+/*
+* DefaultController just sets throttle to motor_throttle, a parameter passed to it on its contructor
+*/
+pub struct DefaultController {
+    motor_throttle: f32,
+}
+
+impl DefaultController {
+    pub fn new(motor_speed: f32) -> DefaultController {
+        return DefaultController {
+            motor_throttle: motor_speed,
+        };
+    }
+}
+
+impl DroneController for DefaultController {
+    fn set_time(&mut self, _time: f32) {}
+    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
+    fn get_motor_throttles(&mut self) -> [f32; 4] {
+        return [self.motor_throttle; 4];
+    }
+
+    /*
+     * These should be implemented like this for all the classes that implement DroneController
+     * Works as an example implementation
+     */
+    fn as_any(&self) -> &dyn Any {
+        self
+    }
+    fn as_mut_any(&mut self) -> &mut dyn Any {
+        self
+    }
+}
+
+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: [
+                na::point![5.0, 5.0, 0.0],
+                na::point![5.0, -5.0, 0.0],
+                na::point![-5.0, -5.0, 0.0],
+                na::point![-5.0, 5.0, 0.0],
+            ],
+            max_thrust: 2.6,
+            max_torque: 0.5,
+            time_constant: 0.01,
+        }
+    }
+}

src/input.rs

@@ -0,0 +1,172 @@
+use std::fs::File;
+use std::io::{Read, Write};
+
+#[repr(C)]
+#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
+pub struct JoystickInput {
+    pub throttle_input: f32,
+    pub roll_input: f32,
+    pub yaw_input: f32,
+    pub pitch_input: f32,
+}
+
+#[repr(C)]
+#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
+pub struct PositionInput {
+    pub lat: f32,
+    pub long: f32,
+    pub alt: f32,
+}
+
+#[repr(C)]
+#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
+pub struct AccelerationInput {
+    pub x: f32,
+    pub y: f32,
+    pub z: f32,
+}
+
+#[repr(C)]
+#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
+pub struct VelocityInput {
+    pub x: f32,
+    pub y: f32,
+    pub z: f32,
+}
+
+#[repr(C)]
+#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
+pub struct RotationInput {
+    pub roll: f32,
+    pub yaw: f32,
+    pub pitch: f32,
+}
+
+#[repr(C)]
+#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
+pub enum ModeInput {
+    #[default]
+    Acro,
+    Rotation,
+    Acceleration,
+    Velocity,
+    Navigation,
+}
+
+#[repr(C)]
+#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
+pub struct Input {
+    #[serde(default)]
+    pub joystick: JoystickInput,
+    #[serde(default)]
+    pub acceleration: AccelerationInput,
+    #[serde(default)]
+    pub rotation: RotationInput,
+    #[serde(default)]
+    pub velocity: VelocityInput,
+    #[serde(default)]
+    pub position: PositionInput,
+
+    pub mode: ModeInput,
+}
+
+#[derive(Default, Clone, Copy, serde::Serialize, serde::Deserialize)]
+pub struct InputRecord {
+    input: Input,
+    time: f32,
+}
+
+#[derive(Default, Clone, serde::Serialize, serde::Deserialize)]
+pub struct InputRecording {
+    records: Vec<InputRecord>,
+}
+
+impl InputRecording {
+    pub fn save_to_file(&self, path: &str) -> Result<(), Box<dyn std::error::Error>> {
+        let json = serde_json::to_string_pretty(self)?;
+        let mut file = File::create(path)?;
+        file.write_all(json.as_bytes())?;
+        Ok(())
+    }
+
+    pub fn load_from_file(path: &str) -> Result<Self, Box<dyn std::error::Error>> {
+        let mut file = File::open(path)?;
+        let mut contents = String::new();
+        file.read_to_string(&mut contents)?;
+        let recording: Self = serde_json::from_str(&contents)?;
+        Ok(recording)
+    }
+
+    pub fn has_ended(&self, time: f32) -> bool {
+        return self.records.last().unwrap_or(&InputRecord::default()).time < time;
+    }
+    pub fn get_input(&self, time: f32) -> Input {
+        /*
+         * Binary search returns index to element as OK, or where the element could be placed to
+         * keep order as Err, so if result is Ok return that input, if its Err, return the previous
+         * input if it exists, if it doesn't (because time is before the first action in the
+         * recorded sequence, return an empty input)
+         */
+        let res = self
+            .records
+            .binary_search_by(|probe| probe.time.total_cmp(&time));
+        match res {
+            Ok(res) => {
+                return self
+                    .records
+                    .get(res)
+                    .unwrap_or(&InputRecord::default())
+                    .input;
+            }
+            Err(mut res) => {
+                if res > 0 {
+                    res -= 1;
+                }
+                return self
+                    .records
+                    .get(0.max(res))
+                    .unwrap_or(&InputRecord::default())
+                    .input;
+            }
+        }
+    }
+
+    /*
+     * Current time should always be larger thant the last input records time.
+     * This method is made for recording inputs in real time, not for retroactively adding
+     *
+     * Returns the addded Joystick Input
+     */
+    pub fn add_input(&mut self, current_time: f32, inp: Input) -> Input {
+        let input = inp;
+        let last_input = self.records.last();
+        match last_input {
+            Some(last_record) => {
+                if last_record.input != input {
+                    self.records.push(InputRecord {
+                        input,
+                        time: current_time,
+                    });
+                }
+            }
+            None => {
+                self.records.push(InputRecord {
+                    input,
+                    time: current_time,
+                });
+            }
+        }
+        return input;
+    }
+}
+
+impl JoystickInput {
+    pub fn clamp(&self) -> JoystickInput {
+        return JoystickInput {
+            throttle_input: self.throttle_input.clamp(0.0, 1.0),
+            yaw_input: self.yaw_input.clamp(-1.0, 1.0),
+            pitch_input: self.pitch_input.clamp(-1.0, 1.0),
+            roll_input: self.roll_input.clamp(-1.0, 1.0),
+        };
+    }
+}

src/lib.rs

@@ -0,0 +1,115 @@
+use nalgebra::{self as na, Quaternion, Unit};
+
+pub mod config;
+pub mod controller;
+pub mod input;
+pub mod stacked;
+
+pub use config::*;
+pub use controller::*;
+pub use input::*;
+pub use stacked::*;
+
+#[unsafe(no_mangle)]
+pub extern "C" fn create(c: config::ControllerConfig) -> *mut StackedController {
+    return Box::into_raw(Box::new(StackedController::new(c)));
+}
+
+
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn destroy(ptr: *mut ControllerConfig) {
+    assert!(!ptr.is_null());
+    let _x = unsafe { Box::from_raw(ptr) };
+}
+
+#[repr(C)]
+pub struct MotorThrottles {
+    pub values: [f32; 4],
+}
+
+#[repr(C)]
+pub struct Vec3C {
+    pub x: f32,
+    pub y: f32,
+    pub z: f32,
+}
+
+impl Into<na::Vector3<f32>> for Vec3C {
+    fn into(self) -> na::Vector3<f32> {
+        na::vector![self.x, self.y, self.z]
+    }
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn get_throttles(ptr: *mut StackedController) -> MotorThrottles {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    return MotorThrottles {
+        values: controller.get_motor_throttles(),
+    };
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_time(ptr: *mut StackedController, time: f32) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_time(time);
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_input(ptr: *mut StackedController, inp: Input) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_input(inp);
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_rot(ptr: *mut StackedController, parts: Vec3C, scalar: f32) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    let quat: na::UnitQuaternion<f32> =
+        na::UnitQuaternion::from_quaternion(Quaternion::from_parts(scalar, parts.into()));
+    controller.set_rotation(quat);
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_angvel(ptr: *mut StackedController, angvel: Vec3C) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_angular_velocity(angvel.into());
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_linvel(ptr: *mut StackedController, vel: Vec3C) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_linvel(vel.into());
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn set_cur_pos(ptr: *mut StackedController, pos: Vec3C) {
+    let controller = unsafe {
+        assert!(!ptr.is_null());
+        &mut *ptr
+    };
+
+    controller.set_pos(pos.into());
+}

src/stacked.rs

@@ -0,0 +1,159 @@
+#![allow(dead_code)]
+
+use nalgebra as na;
+use std::{any::Any, f32};
+
+use crate::input::ModeInput;
+use crate::stacked::modules::ModuleRuntime;
+use crate::{input::Input, DroneController};
+
+use crate::config::*;
+
+pub mod mixer;
+pub mod modules;
+
+use mixer::MotorMixer;
+use modules::*;
+
+#[derive(Default)]
+pub struct DroneState {
+    pub rotation: na::UnitQuaternion<f32>,
+    pub angvel: na::Vector3<f32>,
+    pub linvel: na::Vector3<f32>,
+    pub position: na::Vector3<f32>,
+    pub mass: f32,
+}
+
+pub struct StackedController {
+    pub rotation_rt: ModuleRuntime<RotationController>,
+    pub rate_rt: ModuleRuntime<AngularRateController>,
+    pub linaccel_rt: ModuleRuntime<AccelerationController>,
+    pub linvel_rt: ModuleRuntime<VelocityController>,
+
+    mixer: MotorMixer,
+    config: ControllerConfig,
+
+    // State
+    drone_state: DroneState,
+    input: Input,
+    last_time: f32,
+    current_time: f32,
+}
+
+impl StackedController {
+    pub fn new(config: ControllerConfig) -> Self {
+        let min_throttle = 0.0;
+        let max_throttle = 1.0;
+
+        let lin_vel_ctrl = VelocityController::new(PidProcessor::new(&config.stack.linvel_pid));
+        let lin_accel_ctrl =
+            AccelerationController::new(max_throttle, min_throttle, config.max_thrust);
+        let rotation_ctrl = RotationController::new(PidProcessor::new(&config.stack.rotation_pid));
+        let rate_ctrl = AngularRateController::new(PidProcessor::new(&config.stack.rate_pid));
+
+        Self {
+            linaccel_rt: ModuleRuntime::new(lin_accel_ctrl, config.stack.rotation_pid.frequency),
+            rotation_rt: ModuleRuntime::new(rotation_ctrl, config.stack.rotation_pid.frequency),
+            rate_rt: ModuleRuntime::new(rate_ctrl, config.stack.rate_pid.frequency),
+            linvel_rt: ModuleRuntime::new(lin_vel_ctrl, config.stack.linvel_pid.frequency),
+            mixer: MotorMixer {
+                motor_map: config.motor_map,
+                min_throttle: 0.0,
+                max_throttle: 1.0,
+                mixing_mode: mixer::MotorMixingMode::default(),
+            },
+
+            drone_state: DroneState {
+                rotation: na::UnitQuaternion::identity(),
+                mass: config.mass,
+                ..Default::default()
+            },
+            input: Input::default(),
+            last_time: 0.0,
+            current_time: 0.0,
+
+            config: config, // Keep config copy for future uses
+        }
+    }
+
+    pub fn set_input(&mut self, inp: Input) {
+        self.input = inp;
+    }
+}
+
+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.angvel = vel;
+    }
+
+    fn set_linvel(&mut self, vel: na::Vector3<f32>) {
+        self.drone_state.linvel = vel;
+    }
+
+    fn set_pos(&mut self, pos: na::Vector3<f32>) {
+        self.drone_state.position = pos;
+    }
+
+    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 frame_dt = (self.current_time - self.last_time).max(0.0);
+        let mut throttle = self.input.joystick.throttle_input;
+
+        let angular_rate_setpoint = if self.input.mode != ModeInput::Acro {
+            let rotation_setpoint = if self.input.mode != ModeInput::Rotation {
+                let lin_accel_setpoint = if self.input.mode != ModeInput::Acceleration {
+                    panic!("Not Implemented")
+                } else {
+                    Acceleration(na::vector![
+                        self.input.acceleration.x,
+                        self.input.acceleration.y,
+                        self.input.acceleration.z
+                    ])
+                };
+                let ret = self
+                    .linaccel_rt
+                    .update(lin_accel_setpoint, &self.drone_state, frame_dt);
+                throttle = ret.1 .0;
+                ret.0
+            } else {
+                // println!("Rotation!");
+                Rotation(na::vector![
+                    self.input.rotation.roll,
+                    self.input.rotation.pitch,
+                    self.input.rotation.yaw,
+                ])
+            };
+            self.rotation_rt
+                .update(rotation_setpoint, &self.drone_state, frame_dt)
+        } else {
+            AngularRate(
+                na::vector![
+                    self.input.joystick.roll_input,
+                    self.input.joystick.pitch_input,
+                    self.input.joystick.yaw_input,
+                ] * self.config.stack.max_rate,
+            )
+        };
+
+        let torque = self
+            .rate_rt
+            .update(angular_rate_setpoint, &self.drone_state, frame_dt);
+
+        self.mixer.mix(throttle, torque.0)
+    }
+
+    fn as_any(&self) -> &dyn Any {
+        self
+    }
+    fn as_mut_any(&mut self) -> &mut dyn Any {
+        self
+    }
+}

src/stacked/mixer.rs

@@ -0,0 +1,234 @@
+use nalgebra as na;
+
+pub enum MotorMixingMode {
+    ThrottleAuthority,
+    NoTorqueScalling,
+    ThrottleAuthorityReasonable { min_scale: f32 },
+}
+impl Default for MotorMixingMode {
+    fn default() -> Self {
+        Self::ThrottleAuthorityReasonable { min_scale: 0.1 }
+    }
+}
+
+pub struct MotorMixer {
+    pub motor_map: [[f32; 3]; 4], // roll, pitch, yaw
+    pub min_throttle: f32,
+    pub max_throttle: f32,
+    pub mixing_mode: MotorMixingMode,
+}
+
+impl MotorMixer {
+    pub fn mix(&self, throttle: f32, torque: na::Vector3<f32>) -> [f32; 4] {
+        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;
+            }
+        }
+    }
+
+    fn compute_delta(&self, torque: na::Vector3<f32>) -> [f32; 4] {
+        let mut delta = [0.0f32; 4];
+
+        for i in 0..4 {
+            delta[i] = self.motor_map[i][0] * torque.x
+                + self.motor_map[i][1] * torque.y
+                + self.motor_map[i][2] * torque.z;
+        }
+
+        return delta;
+    }
+
+    /// Unlike ThrottleAuthority, this implementation has a minimum scale.
+    /// That makes sure our torque is never below min_scale * torque.
+    ///
+    /// Throttle still has authority, this is, torque is limited by the throttle, but within reason, which makes it "reasonable"
+    ///
+    /// Avoids the downside of the previous implementation: At 100% Throttle, Torque would always be 0. This lowers the actual throttle.
+    pub fn mix_throttle_authority_reasonable(
+        &self,
+        throttle: f32,
+        torque: na::Vector3<f32>,
+        min_scale: f32,
+    ) -> ([f32; 4], bool) {
+        let mut delta = self.compute_delta(torque);
+        let throttle_range_len = self.max_throttle - self.min_throttle;
+
+        delta.iter_mut().for_each(|x| *x *= 0.25);
+
+        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);
+
+        let mut max_delta = scale * delta.into_iter().reduce(f32::max).unwrap_or(0.0);
+        let mut min_delta = scale * delta.into_iter().reduce(f32::min).unwrap_or(0.0);
+        let delta_dif = max_delta - min_delta;
+
+        if delta_dif > throttle_range_len {
+            scale *= throttle_range_len / delta_dif * 0.99;
+            max_delta *= throttle_range_len / delta_dif * 0.99;
+            min_delta *= throttle_range_len / delta_dif * 0.99;
+        }
+
+        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] * scale;
+        }
+
+        let saturated = scale < 1.0;
+
+        (motors, saturated)
+    }
+
+    pub fn mix_attitude_authority(&self, throttle: f32, torque: na::Vector3<f32>) -> [f32; 4] {
+        return self
+            .mix_throttle_authority_reasonable(throttle, torque, 1.0)
+            .0;
+    }
+
+    ///
+    /// Made by Chatgpt, unreliable as hell. Deltas try to fit within throttle,
+    /// so if throttle is the max, will simply not apply deltas. Shouldn't be used.
+    ///
+    /// Will actually kind of work if throttle_min + delta < throttle < throttle_max - delta,
+    /// Where delta is a value of headroom added so that we always have room to create torque
+    ///
+    pub fn mix_throttle_authority(
+        &self,
+        throttle: f32,
+        torque: na::Vector3<f32>,
+    ) -> ([f32; 4], bool) {
+        let mut delta = [0.0f32; 4];
+
+        // 1. Torque-only contribution
+        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;
+        }
+
+        // 2. Compute allowable scaling
+        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(0.0, 1.0);
+
+        // 3. Apply
+        let mut motors = [0.0f32; 4];
+        for i in 0..4 {
+            motors[i] = throttle + scale * delta[i];
+        }
+
+        let saturated = scale < 1.0;
+        (motors, saturated)
+    }
+
+    /// Bad, not mine, used for testing and comparison
+    pub fn mix_no_torque_scalling(
+        &self,
+        throttle: f32,
+        torque: na::Vector3<f32>,
+    ) -> ([f32; 4], bool) {
+        let mut motors = [0.0f32; 4];
+
+        // --------------------------------------------------
+        // 1. Raw mix: throttle + torque deltas
+        // --------------------------------------------------
+        for i in 0..4 {
+            let delta = self.motor_map[i][0] * torque.x
+                + self.motor_map[i][1] * torque.y
+                + self.motor_map[i][2] * torque.z;
+
+            motors[i] = throttle + delta;
+        }
+
+        // --------------------------------------------------
+        // 2. Find saturation
+        // --------------------------------------------------
+        let mut max_motor = f32::MIN;
+        let mut min_motor = f32::MAX;
+
+        for &m in motors.iter() {
+            max_motor = max_motor.max(m);
+            min_motor = min_motor.min(m);
+        }
+
+        let mut saturated = false;
+
+        // --------------------------------------------------
+        // 3. Thrust-priority correction (shift down)
+        //    Preserves all torque differences
+        // --------------------------------------------------
+        if max_motor > self.max_throttle {
+            let excess = max_motor - self.max_throttle;
+            for m in motors.iter_mut() {
+                *m -= excess;
+            }
+            saturated = true;
+        }
+
+        // --------------------------------------------------
+        // 4. Recompute minimum after shift
+        // --------------------------------------------------
+        min_motor = f32::MAX;
+        for &m in motors.iter() {
+            min_motor = min_motor.min(m);
+        }
+
+        // --------------------------------------------------
+        // 5. Bottom correction (shift up if possible)
+        //    Still preserves torque
+        // --------------------------------------------------
+        if min_motor < self.min_throttle {
+            let deficit = self.min_throttle - min_motor;
+            for m in motors.iter_mut() {
+                *m += deficit;
+            }
+            saturated = true;
+        }
+
+        // --------------------------------------------------
+        // 6. Final clamp (last resort — torque may be lost)
+        // --------------------------------------------------
+        for m in motors.iter_mut() {
+            if *m > self.max_throttle {
+                *m = self.max_throttle;
+                saturated = true;
+            } else if *m < self.min_throttle {
+                *m = self.min_throttle;
+                saturated = true;
+            }
+        }
+
+        (motors, saturated)
+    }
+}

src/stacked/modules.rs

@@ -0,0 +1,117 @@
+use crate::DroneState;
+use crate::PidConfig;
+use nalgebra as na;
+
+pub mod acceleration;
+pub mod angular_rate;
+pub mod rotation;
+pub mod velocity;
+
+pub use acceleration::AccelerationController;
+pub use angular_rate::AngularRateController;
+pub use rotation::RotationController;
+pub use velocity::VelocityController;
+
+pub trait ControllerModule {
+    type Input: Clone + Default;
+    type Output: Clone + Default;
+
+    fn process(&mut self, input: Self::Input, state: &DroneState, dt: f32) -> Self::Output;
+}
+
+pub struct ModuleRuntime<C>
+where
+    C: ControllerModule,
+{
+    pub module: C,
+    pub target_dt: f32,
+    pub accumulated_time: f32,
+    pub last_output: Option<C::Output>,
+}
+
+impl<C> ModuleRuntime<C>
+where
+    C: ControllerModule,
+{
+    pub fn update(&mut self, input: C::Input, state: &DroneState, dt: f32) -> C::Output {
+        self.accumulated_time += dt;
+
+        if self.accumulated_time >= self.target_dt {
+            self.accumulated_time = 0.0;
+
+            let output = self.module.process(input, state, self.target_dt);
+            self.last_output = Some(output.clone());
+            output
+        } else {
+            self.last_output
+                .as_ref()
+                .unwrap_or(&C::Output::default())
+                .clone()
+        }
+    }
+    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 Acceleration(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
+
+#[derive(Clone, Copy, Debug, Default)]
+pub struct Throttle(pub f32);
+
+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 {
+    pub 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(),
+        }
+    }
+
+    pub 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/stacked/modules/acceleration.rs

@@ -0,0 +1,47 @@
+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))
+    }
+}

src/stacked/modules/angular_rate.rs

@@ -0,0 +1,25 @@
+use crate::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.angvel, dt);
+
+        Torque(output)
+    }
+}

src/stacked/modules/rotation.rs

@@ -0,0 +1,29 @@
+use nalgebra::UnitQuaternion;
+
+use crate::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/stacked/modules/velocity.rs

@@ -0,0 +1,23 @@
+use crate::stacked::modules::*;
+
+pub struct VelocityController {
+    pid: PidProcessor,
+}
+
+impl VelocityController {
+    pub fn new(pid: PidProcessor) -> Self {
+        Self { pid }
+    }
+}
+
+impl ControllerModule for VelocityController {
+    type Input = Velocity;
+    type Output = Acceleration;
+
+    fn process(&mut self, input: Velocity, state: &DroneState, dt: f32) -> Acceleration {
+        let target_rate = input.0;
+
+        let output = self.pid.update(target_rate, state.linvel, dt);
+        Acceleration(output)
+    }
+}