pull liftoff quad references from quadrotor interface implementation

src/config.rs

@@ -4,7 +4,7 @@
 //! The configuration is loaded from a YAML file using the serde library.
 //! The configuration is then used to initialize the simulation, quadrotor, PID controller, IMU, maze, camera, mesh, and planner schedule.
 
-use nalgebra::{Matrix3, Vector3};
+use nalgebra::Matrix3;
 
 use crate::SimulationError;
 
@@ -46,7 +46,6 @@ pub struct Config {
 /// Vehicle Specifig configuration
 pub enum QuadrotorConfigurations {
     Peng(QuadrotorConfig),
-    Liftoff(LiftoffQuadrotorConfig),
 }
 
 #[derive(serde::Deserialize)]
@@ -60,9 +59,11 @@ pub struct PlannerStep {
     pub params: serde_yaml::Value,
 }
 
-#[derive(Clone, Default, serde::Deserialize)]
+#[derive(Clone, serde::Deserialize)]
 /// Configuration for the simulation
 pub struct SimulationConfig {
+    /// Gravity in m/s^2
+    pub gravity: f32,
     /// Control frequency in Hz
     pub control_frequency: usize,
     /// Simulation frequency in Hz
@@ -77,14 +78,26 @@ pub struct SimulationConfig {
     pub use_rk4_for_dynamics_update: bool,
 }
 
+impl Default for SimulationConfig {
+    fn default() -> Self {
+        SimulationConfig {
+            gravity: 9.81,
+            control_frequency: 200,
+            simulation_frequency: 1000,
+            log_frequency: 70,
+            duration: 70.0,
+            use_rk4_for_dynamics_control: false,
+            use_rk4_for_dynamics_update: false,
+        }
+    }
+}
+
 #[derive(Copy, Clone, Debug, serde::Deserialize)]
 #[serde(default)]
 /// Configuration for the quadrotor
 pub struct QuadrotorConfig {
     /// Mass of the quadrotor in kg
     pub mass: f32,
-    /// Gravity in m/s^2
-    pub gravity: f32,
     /// Drag coefficient in Ns^2/m^2
     pub drag_coefficient: f32,
     /// Inertia matrix in kg*m^2
@@ -101,7 +114,6 @@ impl Default for QuadrotorConfig {
     fn default() -> Self {
         QuadrotorConfig {
             mass: 1.3,
-            gravity: 9.81,
             drag_coefficient: 0.000,
             inertia_matrix: [3.04e-3, 0.0, 0.0, 0.0, 4.55e-3, 0.0, 0.0, 0.0, 2.82e-3],
             max_thrust_kg: 1.3 * 2.5,
@@ -126,59 +138,6 @@ impl QuadrotorConfig {
     }
 }
 
-#[derive(Clone, Debug, serde::Deserialize)]
-#[serde(default)]
-/// Configuration for the quadrotor
-pub struct LiftoffQuadrotorConfig {
-    /// Mass of the quadrotor in kg
-    pub mass: f32,
-    /// Gravity in m/s^2
-    pub gravity: f32,
-    /// Inertia matrix in kg*m^2
-    pub inertia_matrix: [f32; 9],
-    /// Maximum thrust in kilograms
-    pub max_thrust_kg: f32,
-    /// Arm length in meters
-    pub arm_length_m: f32,
-    /// Yaw torque constant
-    pub yaw_torque_constant: f32,
-    /// The IP address where Liftoff is publishing state data
-    pub ip_address: String,
-    pub connection_timeout: tokio::time::Duration,
-    pub max_retry_delay: tokio::time::Duration,
-    pub serial_port: Option<String>,
-    pub baud_rate: u32,
-}
-
-impl Default for LiftoffQuadrotorConfig {
-    fn default() -> Self {
-        LiftoffQuadrotorConfig {
-            mass: 1.3,
-            gravity: 9.81,
-            inertia_matrix: [3.04e-3, 0.0, 0.0, 0.0, 4.55e-3, 0.0, 0.0, 0.0, 2.82e-3],
-            max_thrust_kg: 1.3 * 2.5,
-            arm_length_m: 0.150,
-            yaw_torque_constant: 0.05,
-            ip_address: String::from("0.0.0.0:9001"),
-            connection_timeout: tokio::time::Duration::from_secs(5 * 60),
-            max_retry_delay: tokio::time::Duration::from_secs(30),
-            serial_port: None,
-            baud_rate: 460800,
-        }
-    }
-}
-
-impl LiftoffQuadrotorConfig {
-    /// Calculate all maximum torques and return them as a tuple
-    pub fn max_torques(&self) -> (f32, f32, f32) {
-        let motor_thrust = self.max_thrust_kg / 4.0;
-        // The maximum roll and pitch torques
-        let max_rp_torque = 2.0 * self.arm_length_m * motor_thrust;
-        let yaw_torque = 2.0 * self.yaw_torque_constant * motor_thrust;
-        (max_rp_torque, max_rp_torque, yaw_torque)
-    }
-}
-
 #[derive(serde::Deserialize)]
 /// Configuration for the PID controller
 pub struct PIDControllerConfig {
@@ -285,22 +244,11 @@ mod tests {
         assert_eq!(config.simulation.log_frequency, 20);
         assert_eq!(config.simulation.duration, 70.0);
         assert_eq!(quadrotor_config.mass, 1.3);
-        assert_eq!(quadrotor_config.gravity, 9.81);
         assert_eq!(quadrotor_config.drag_coefficient, 0.0);
         assert_eq!(config.pid_controller.pos_gains.kp, [7.1, 7.1, 11.9]);
         assert_eq!(config.pid_controller.att_gains.kd, [0.13, 0.13, 0.1]);
         assert_eq!(config.pid_controller.pos_max_int, [10.0, 10.0, 10.0]);
         assert_eq!(config.imu.accel_noise_std, 0.02);
         assert_eq!(config.maze.upper_bounds, [4.0, 2.0, 2.0]);
     }
-
-    #[test]
-    fn test_liftoff_config() {
-        let config = Config::from_yaml("tests/testdata/test_liftoff_base.yaml").unwrap();
-        let liftoff_config = match config.quadrotor {
-            QuadrotorConfigurations::Liftoff(liftoff_config) => liftoff_config,
-            _ => panic!("Failed to load Liftoff configuration"),
-        };
-        assert_eq!(liftoff_config.ip_address, "0.0.0.0:9001");
-    }
 }

src/main.rs

@@ -91,34 +91,28 @@ async fn main() -> Result<(), SimulationError> {
         log_maze_tube(rec, &maze)?;
         log_maze_obstacles(rec, &maze)?;
     }
-    let (mut quad, mass, gravity): (Box<dyn QuadrotorInterface>, f32, f32) = match config.quadrotor
-    {
+    let (mut quad, mass): (Box<dyn QuadrotorInterface>, f32) = match config.quadrotor {
         config::QuadrotorConfigurations::Peng(quad_config) => (
             Box::new(Quadrotor::new(
                 1.0 / config.simulation.simulation_frequency as f32,
                 config.simulation.clone(),
                 quad_config.mass,
-                quad_config.gravity,
+                config.simulation.gravity,
                 quad_config.drag_coefficient,
                 quad_config.inertia_matrix,
             )?),
             quad_config.mass,
-            quad_config.gravity,
-        ),
-        config::QuadrotorConfigurations::Liftoff(ref liftoff_quad_config) => (
-            Box::new(LiftoffQuad::new(
-                1.0 / config.simulation.control_frequency as f32,
-                config.simulation.clone(),
-                liftoff_quad_config.clone(),
-            )?),
-            liftoff_quad_config.mass,
-            liftoff_quad_config.gravity,
         ),
+        _ => {
+            return Err(SimulationError::OtherError(
+                "Unsupported quadrotor type".to_string(),
+            ))
+        }
     };
 
     println!(
         "[\x1b[32mINFO\x1b[0m peng_quad] Quadrotor: {:?} {:?}",
-        mass, gravity
+        mass, config.simulation.gravity
     );
     let _pos_gains = config.pid_controller.pos_gains;
     let _att_gains = config.pid_controller.att_gains;
@@ -128,7 +122,7 @@ async fn main() -> Result<(), SimulationError> {
         config.pid_controller.pos_max_int,
         config.pid_controller.att_max_int,
         mass,
-        gravity,
+        config.simulation.gravity,
     );
     log::info!("Starting simulation...");
     let mut i = 0;