From f261a12bc595f0e1a1a27c5a01841501e973566f Mon Sep 17 00:00:00 2001
From: franchioping <franchioping@galard.uk>
Date: Thu, 5 Feb 2026 21:12:54 +0000
Subject: [PATCH] stacked controller working base (ACRO ONLY). Motor mixer
 needs to be improved. throttle has authority over torque at the moment

---
 src/drone/stacked.rs       | 2 +-
 src/drone/stacked/mixer.rs | 8 ++++----
 src/main.rs                | 1 -
 3 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/drone/stacked.rs b/src/drone/stacked.rs
index 82a1a78..54083a6 100644
--- a/src/drone/stacked.rs
+++ b/src/drone/stacked.rs
@@ -100,7 +100,7 @@ impl DroneController for StackedController {
             setpoint = module.process(setpoint, &self.drone_state, dt, is_first_layer);
         }
 
-        return self.mixer.mix(self.input.throttle_input, setpoint).0;
+        return self.mixer.mix_throttle_authority(0.5, setpoint).0;
     }
 
     fn as_any(&self) -> &dyn Any {
diff --git a/src/drone/stacked/mixer.rs b/src/drone/stacked/mixer.rs
index 2393a22..5ccaf98 100644
--- a/src/drone/stacked/mixer.rs
+++ b/src/drone/stacked/mixer.rs
@@ -7,14 +7,14 @@ pub struct MotorMixer {
 }
 
 impl MotorMixer {
-    pub fn mix(&self, throttle: f32, desired: na::Vector3<f32>) -> ([f32; 4], bool) {
+    pub fn mix(&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] * desired.x
-                + self.motor_map[i][1] * desired.y
-                + self.motor_map[i][2] * desired.z;
+            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
diff --git a/src/main.rs b/src/main.rs
index 6e3352b..a5774a9 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -20,7 +20,6 @@ const INPUTS_DIR: &str = "inputs/";
 const CONFIGS_DIR: &str = "configurations/";
 const RESULTS_DIR: &str = "results/";
 
-use std::env;
 use std::path::PathBuf;
 use std::thread::JoinHandle;