stacked controller working base (ACRO ONLY). Motor mixer needs to be improved. throttle has authority over torque at the moment

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 {

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
+            delta[i] = self.motor_map[i][0] * torque.x
-                + self.motor_map[i][1] * desired.y
+                + self.motor_map[i][1] * torque.y
-                + self.motor_map[i][2] * desired.z;
+                + self.motor_map[i][2] * torque.z;
         }
 
         // 2. Compute allowable scaling

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;