From 2e8842ca24c599996cd9f0b58be7bd65e9fdaa92 Mon Sep 17 00:00:00 2001
From: Franchioping <franchioping@gmail.com>
Date: Tue, 9 Dec 2025 20:37:03 +0000
Subject: [PATCH] fogot files

---
 src/drone/fpvcontroller.rs | 83 ++++++++++++++++++++++++++++++++++++++
 src/drone/pidcontroller.rs | 81 +++++++++++++++++++++++++++++++++++++
 2 files changed, 164 insertions(+)
 create mode 100644 src/drone/fpvcontroller.rs
 create mode 100644 src/drone/pidcontroller.rs

diff --git a/src/drone/fpvcontroller.rs b/src/drone/fpvcontroller.rs
new file mode 100644
index 0000000..0f93cb8
--- /dev/null
+++ b/src/drone/fpvcontroller.rs
@@ -0,0 +1,83 @@
+use rapier3d::prelude as rp;
+use std::any::Any;
+
+use crate::drone::controller::DroneController;
+pub use crate::drone::controller::JoystickInput;
+use crate::drone::MotorCharacteristics;
+
+pub struct FPVController {
+    input: JoystickInput,
+    rate_yaw: f32,
+    rate_pitch: f32,
+    rate_roll: f32,
+}
+
+impl FPVController {
+    pub fn set_input(&mut self, input: JoystickInput) {
+        self.input = input;
+    }
+}
+
+impl Default for FPVController {
+    fn default() -> Self {
+        Self {
+            input: JoystickInput::default(),
+            rate_yaw: 1.0,
+            rate_pitch: 1.0,
+            rate_roll: 1.0,
+        }
+    }
+}
+
+impl DroneController for FPVController {
+    fn set_position(&mut self, _position: rp::Isometry<f32>) {}
+    fn set_time(&mut self, _time: f32) {}
+    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
+    fn get_motor_throttles(&self) -> [f32; 4] {
+        let yaw = self.rate_yaw * self.input.yaw_input;
+        let roll = self.rate_roll * self.input.roll_input;
+        let pitch = self.rate_pitch * self.input.pitch_input;
+        let throttle = self.input.throttle_input;
+
+        /*
+         * Motor position indices
+         *    ^ - Front
+         *    |
+         *    |
+         * 1 --- 0
+         * |     |
+         * |     |
+         * 2 --- 3
+         */
+
+        let mut motors = [
+            throttle + pitch - roll + yaw,
+            throttle + pitch + roll - yaw,
+            throttle - pitch + roll + yaw,
+            throttle - pitch - roll - yaw,
+        ];
+        // motors.max
+        let max = motors
+            .iter()
+            .copied()
+            .max_by(|a, b| a.total_cmp(b))
+            .unwrap_or(0.0);
+        if max > 1.0 {
+            for v in motors.iter_mut() {
+                *v /= max;
+            }
+        }
+        return motors;
+    }
+
+    /*
+     * 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
+    }
+}
diff --git a/src/drone/pidcontroller.rs b/src/drone/pidcontroller.rs
new file mode 100644
index 0000000..9eaa70e
--- /dev/null
+++ b/src/drone/pidcontroller.rs
@@ -0,0 +1,81 @@
+use glam::{vec3, EulerRot};
+use nalgebra::{self as na, Vector3};
+use rapier3d::prelude as rp;
+use std::{any::Any, f32};
+
+use crate::drone::controller::DroneController;
+pub use crate::drone::controller::JoystickInput;
+use crate::drone::MotorCharacteristics;
+
+#[derive(Default)]
+pub struct PIDControllerState {
+    last_time: f32,
+    current_time: f32,
+    last_rotation: na::Unit<na::Quaternion<f32>>,
+    current_rotation: na::Unit<na::Quaternion<f32>>,
+}
+
+impl PIDControllerState {
+    pub fn dt(&self) -> f32 {
+        return self.last_time - self.current_time;
+    }
+}
+
+pub struct PIDController {
+    input: JoystickInput,
+    pub target_rate: f32,
+    state: PIDControllerState,
+}
+
+impl Default for PIDController {
+    fn default() -> Self {
+        Self {
+            input: JoystickInput::default(),
+            target_rate: f32::consts::PI,
+            state: Default::default(),
+        }
+    }
+}
+
+impl PIDController {
+    pub fn set_input(&mut self, input: JoystickInput) {
+        self.input = input;
+    }
+    pub fn get_angular_velocity(&self) {
+        let dt: f32 = self.state.dt();
+        let angles =
+            (self.state.current_rotation.inverse() * self.state.last_rotation).euler_angles();
+        // roll,pitch.yaw
+        let vel_roll = angles.0 / dt;
+        let vel_pitch = angles.1 / dt;
+        let vel_yaw = angles.2 / dt;
+        println!("{},{},{}", vel_roll, vel_pitch, vel_yaw);
+    }
+}
+
+impl DroneController for PIDController {
+    fn set_position(&mut self, position: rp::Isometry<f32>) {
+        self.state.last_rotation = self.state.current_rotation;
+        self.state.current_rotation = position.rotation;
+    }
+    fn set_time(&mut self, time: f32) {
+        self.state.last_time = self.state.current_time;
+        self.state.current_time = time;
+    }
+    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
+    fn get_motor_throttles(&self) -> [f32; 4] {
+        self.get_angular_velocity();
+        return [1.0, 1.0, 1.0, 1.0];
+    }
+
+    /*
+     * 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
+    }
+}