From 11a55db27127a10a99439cde27a04489bb7318d2 Mon Sep 17 00:00:00 2001
From: franchioping <franchioping@galard.uk>
Date: Fri, 3 Apr 2026 01:38:49 +0100
Subject: [PATCH] position controller

---
 src/stacked.rs                  |  4 ++--
 src/stacked/mixer.rs            |  6 +++---
 src/stacked/modules/position.rs | 26 ++++++++++++++++++++++++++
 3 files changed, 31 insertions(+), 5 deletions(-)
 create mode 100644 src/stacked/modules/position.rs

diff --git a/src/stacked.rs b/src/stacked.rs
index b9c6fb0..e3ed2d7 100644
--- a/src/stacked.rs
+++ b/src/stacked.rs
@@ -64,8 +64,8 @@ impl StackedController {
             position_rt: ModuleRuntime::new(position_ctrl, config.stack.position_pid.frequency),
             mixer: MotorMixer {
                 motor_map: config.motor_map,
-                min_throttle: 0.0,
-                max_throttle: 1.0,
+                min_throttle: min_throttle,
+                max_throttle: max_throttle,
                 mixing_mode: mixer::MotorMixingMode::default(),
             },
 
diff --git a/src/stacked/mixer.rs b/src/stacked/mixer.rs
index c708ce4..581311e 100644
--- a/src/stacked/mixer.rs
+++ b/src/stacked/mixer.rs
@@ -82,9 +82,9 @@ impl MotorMixer {
         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;
+            scale *= throttle_range_len / delta_dif * 0.98;
+            max_delta *= throttle_range_len / delta_dif * 0.98;
+            min_delta *= throttle_range_len / delta_dif * 0.98;
         }
 
         let lim_throttle = throttle.clamp(
diff --git a/src/stacked/modules/position.rs b/src/stacked/modules/position.rs
new file mode 100644
index 0000000..b6d0303
--- /dev/null
+++ b/src/stacked/modules/position.rs
@@ -0,0 +1,26 @@
+use crate::stacked::modules::*;
+
+pub struct PositionController {
+    pid: PidProcessor,
+    max_linvel: f32,
+}
+
+impl PositionController {
+    pub fn new(pid: PidProcessor, max_linvel: f32) -> Self {
+        Self { pid, max_linvel }
+    }
+}
+
+impl ControllerModule for PositionController {
+    type Input = Position;
+    type Output = Velocity;
+
+    fn process(&mut self, input: Position, state: &DroneState, dt: f32) -> Velocity {
+        let target_rate = input.0;
+
+        let output: na::Vector3<f32> = self.pid.update(target_rate, state.position, dt);
+        let norm = output.norm().clamp(0.0, self.max_linvel);
+
+        Velocity(output.normalize() * norm)
+    }
+}