before fix with new files

src/drone/stacked/mixer.rs

@@ -0,0 +1,42 @@
+use nalgebra as na;
+
+pub struct MotorMixer {
+    pub motor_map: [[f32; 3]; 4], // roll, yaw, pitch
+    pub min_throttle: f32,
+    pub max_throttle: f32,
+}
+
+impl MotorMixer {
+    pub fn mix(&self, throttle: f32, desired: 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;
+        }
+
+        // 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)
+    }
+}

src/drone/stacked/modules.rs

@@ -0,0 +1,94 @@
+use crate::config::PidConfig;
+use crate::drone::stacked::DroneState;
+use nalgebra as na;
+
+pub enum ControllerModule {
+    Rate {
+        processor: PidProcessor,
+        max_rate: f32,
+    },
+    Angle {
+        processor: PidProcessor,
+        max_angle: f32,
+    },
+}
+
+impl ControllerModule {
+    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 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/main_test.rs

@@ -0,0 +1,95 @@
+use macroquad::prelude as mq;
+use rapier3d::prelude::*;
+
+mod engine;
+use engine::*;
+
+mod camera;
+mod drone;
+mod rendering;
+
+mod config;
+mod graphics_util;
+
+use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer};
+
+fn window_conf() -> mq::Conf {
+    mq::Conf {
+        window_title: "RustDroneSim".to_owned(),
+        window_resizable: true,
+        // fullscreen: true,
+        platform: mq::miniquad::conf::Platform {
+            // linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
+            ..Default::default()
+        },
+        ..Default::default()
+    }
+}
+
+#[macroquad::main(window_conf)]
+async fn main() {
+    /* World Setup */
+    let mut world = World::new(600.0);
+
+    world.register_free_collider(
+        ColliderBuilder::cuboid(30.0, 1.0, 30.0)
+            .translation(vector![0.0, -2.0, 0.0])
+            .restitution(0.5)
+            .build(),
+        None,
+    );
+
+    let mut drone_controller = drone::pidcontroller::PIDController::default();
+    drone_controller.set_input(JoystickInput {
+        throttle_input: 0.2,
+        yaw_input: 0.2,
+        roll_input: 0.2,
+        pitch_input: 0.0,
+    });
+    let mut drone_obj = drone::Drone::new(
+        &mut world,
+        Box::new(drone_controller),
+        drone::MotorCharacteristics {
+            max_thrust: 2.6,
+            max_torque: 0.5,
+            ..Default::default()
+        },
+        0.350,
+    );
+
+    /* Renderer Setup */
+    let camera = camera::AttachedCamera::new(
+        drone_obj.rb_handle,
+        vector![1.0, 0.0, 0.0],
+        vector![0.5, 0.0, 0.0],
+    );
+    let mut renderer = Renderer::new(Box::new(camera));
+
+    renderer.light.set_location(
+        vector![70.0, 150.0, -90.0].into(),
+        vector![-0.4, -0.7, 0.6].into(),
+    );
+    renderer.update_light(&world);
+
+    loop {
+        renderer.update_camera(&world);
+
+        if mq::is_key_pressed(mq::KeyCode::Q) {
+            break;
+        }
+
+        // Physics
+        world.step();
+        let _ = clearscreen::clear();
+        drone_obj.process_tick(&mut world);
+
+        // Rendering
+        renderer.draw(&mut world);
+
+        if world.tick % (10) == 0 {
+            // renderer.update_light(&world);
+            // world.clear_ofb();
+            mq::next_frame().await;
+        }
+    }
+}