add the quadrotor trait, stub implementations for RC, liftoff quad

src/lib.rs

@@ -48,6 +48,29 @@ pub enum SimulationError {
     #[error("Other error: {0}")]
     OtherError(String),
 }
+
+pub trait Quadrotor {
+    fn control(
+        &mut self,
+        step_number: usize,
+        config: &config::Config,
+        thrust: f32,
+        torque: &Vector3<f32>,
+    );
+
+    fn observe(
+        &self,
+    ) -> Result<
+        (
+            Vector3<f32>,
+            Vector3<f32>,
+            UnitQuaternion<f32>,
+            Vector3<f32>,
+        ),
+        SimulationError,
+    >;
+}
+
 /// Represents a quadrotor with its physical properties and state
 /// # Example
 /// ```
@@ -83,6 +106,56 @@ pub struct SimulatedQuadrotor {
     /// Previous Torque
     pub previous_torque: Vector3<f32>,
 }
+
+impl Quadrotor for SimulatedQuadrotor {
+    fn control(
+        &mut self,
+        step_number: usize,
+        config: &config::Config,
+        thrust: f32,
+        torque: &Vector3<f32>,
+    ) {
+        if step_number
+            % (config.simulation.simulation_frequency / config.simulation.control_frequency)
+            == 0
+        {
+            if config.use_rk4_for_dynamics_control {
+                self.update_dynamics_with_controls_rk4(thrust, &torque);
+            } else {
+                self.update_dynamics_with_controls_euler(thrust, &torque);
+            }
+            self.previous_thrust = thrust;
+            self.previous_torque = *torque;
+        } else {
+            let previous_thrust = self.previous_thrust;
+            let previous_torque = self.previous_torque;
+            if config.use_rk4_for_dynamics_update {
+                self.update_dynamics_with_controls_rk4(previous_thrust, &previous_torque);
+            } else {
+                self.update_dynamics_with_controls_euler(previous_thrust, &previous_torque);
+            }
+        }
+    }
+    fn observe(
+        &self,
+    ) -> Result<
+        (
+            Vector3<f32>,
+            Vector3<f32>,
+            UnitQuaternion<f32>,
+            Vector3<f32>,
+        ),
+        SimulationError,
+    > {
+        return Ok((
+            self.position,
+            self.velocity,
+            self.orientation,
+            self.angular_velocity,
+        ));
+    }
+}
+
 /// Implementation of the Quadrotor struct
 impl SimulatedQuadrotor {
     /// Creates a new Quadrotor with default parameters
@@ -135,35 +208,6 @@ impl SimulatedQuadrotor {
         })
     }
 
-    pub fn control(
-        &mut self,
-        step_number: usize,
-        config: &config::Config,
-        thrust: f32,
-        torque: &Vector3<f32>,
-    ) {
-        if step_number
-            % (config.simulation.simulation_frequency / config.simulation.control_frequency)
-            == 0
-        {
-            if config.use_rk4_for_dynamics_control {
-                self.update_dynamics_with_controls_rk4(thrust, &torque);
-            } else {
-                self.update_dynamics_with_controls_euler(thrust, &torque);
-            }
-            self.previous_thrust = thrust;
-            self.previous_torque = *torque;
-        } else {
-            let previous_thrust = self.previous_thrust;
-            let previous_torque = self.previous_torque;
-            if config.use_rk4_for_dynamics_update {
-                self.update_dynamics_with_controls_rk4(previous_thrust, &previous_torque);
-            } else {
-                self.update_dynamics_with_controls_euler(previous_thrust, &previous_torque);
-            }
-        }
-    }
-
     /// Updates the quadrotor's dynamics with control inputs
     /// # Arguments
     /// * `control_thrust` - The total thrust force applied to the quadrotor

src/liftoff_quad.rs

@@ -0,0 +1,60 @@
+use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3};
+use peng_quad::{Quadrotor, SimulationError};
+
+/// Represents an RC quadrotor
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::Quadrotor;
+/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+/// let quadrotor = RCQuad::new(time_step, mass, inertia_matrix);
+/// ```
+pub struct LiftoffQuad {
+    /// Current position of the quadrotor in 3D space
+    pub position: Vector3<f32>,
+    /// Current velocity of the quadrotor
+    pub velocity: Vector3<f32>,
+    /// Current orientation of the quadrotor
+    pub orientation: UnitQuaternion<f32>,
+    /// Current angular velocity of the quadrotor
+    pub angular_velocity: Vector3<f32>,
+    /// Simulation time step in seconds
+    pub time_step: f32,
+    /// Mass of the quadrotor in kg
+    pub mass: f32,
+    /// Inertia matrix of the quadrotor
+    pub inertia_matrix: Matrix3<f32>,
+    /// Inverse of the inertia matrix
+    pub inertia_matrix_inv: Matrix3<f32>,
+    /// Previous Thrust
+    pub previous_thrust: f32,
+    /// Previous Torque
+    pub previous_torque: Vector3<f32>,
+}
+
+impl Quadrotor for LiftoffQuad {
+    fn control(
+        &mut self,
+        step_number: usize,
+        config: &peng_quad::config::Config,
+        thrust: f32,
+        torque: &Vector3<f32>,
+    ) {
+        todo!("implement control outputs to CyberRC - gamepad mode")
+    }
+
+    fn observe(
+        &self,
+    ) -> Result<
+        (
+            Vector3<f32>,
+            Vector3<f32>,
+            UnitQuaternion<f32>,
+            Vector3<f32>,
+        ),
+        SimulationError,
+    > {
+        todo!("implement state feedback from Liftoff UDP")
+    }
+}

src/main.rs

@@ -5,6 +5,9 @@ use std::thread;
 use std::time::Duration;
 use std::time::Instant;
 
+mod liftoff_quad;
+mod rc_quad;
+
 /// Main function for the simulation
 fn main() -> Result<(), SimulationError> {
     env_logger::builder()

src/rc_quad.rs

@@ -0,0 +1,60 @@
+use nalgebra::{Matrix3, Quaternion, Rotation3, SMatrix, UnitQuaternion, Vector3};
+use peng_quad::{Quadrotor, SimulationError};
+
+/// Represents an RC quadrotor
+/// # Example
+/// ```
+/// use nalgebra::Vector3;
+/// use peng_quad::Quadrotor;
+/// let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
+/// let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
+/// let quadrotor = RCQuad::new(time_step, mass, inertia_matrix);
+/// ```
+pub struct RCQuad {
+    /// Current position of the quadrotor in 3D space
+    pub position: Vector3<f32>,
+    /// Current velocity of the quadrotor
+    pub velocity: Vector3<f32>,
+    /// Current orientation of the quadrotor
+    pub orientation: UnitQuaternion<f32>,
+    /// Current angular velocity of the quadrotor
+    pub angular_velocity: Vector3<f32>,
+    /// Simulation time step in seconds
+    pub time_step: f32,
+    /// Mass of the quadrotor in kg
+    pub mass: f32,
+    /// Inertia matrix of the quadrotor
+    pub inertia_matrix: Matrix3<f32>,
+    /// Inverse of the inertia matrix
+    pub inertia_matrix_inv: Matrix3<f32>,
+    /// Previous Thrust
+    pub previous_thrust: f32,
+    /// Previous Torque
+    pub previous_torque: Vector3<f32>,
+}
+
+impl Quadrotor for RCQuad {
+    fn control(
+        &mut self,
+        step_number: usize,
+        config: &peng_quad::config::Config,
+        thrust: f32,
+        torque: &Vector3<f32>,
+    ) {
+        todo!("implement control outputs to CyberRC - PPM mode")
+    }
+
+    fn observe(
+        &self,
+    ) -> Result<
+        (
+            Vector3<f32>,
+            Vector3<f32>,
+            UnitQuaternion<f32>,
+            Vector3<f32>,
+        ),
+        SimulationError,
+    > {
+        todo!("implement state feedback from Vicon")
+    }
+}