add a realtime configuration option

config/quad.yaml

@@ -3,7 +3,7 @@ render_depth: true # Enable rendering depth
 use_multithreading_depth_rendering: true # Enable multithreading for depth rendering for large resolution (above 32x24)
 use_rk4_for_dynamics_update: false # Enable Runge-Kutta 4th order integration for dynamics, otherwise Euler integration is used
 use_rk4_for_dynamics_control: false # Enable Runge-Kutta 4th order integration for dynamics, otherwise Euler integration is used
-
+real_time: true # Enable real time mode. If not enabled, sim will run in fast time.
 simulation:
   control_frequency: 200 # Frequency of control loop execution (Hz)
   simulation_frequency: 1000 # Frequency of physics simulation updates (Hz)

rust-toolchain.toml

@@ -0,0 +1,2 @@
+[toolchain]
+channel = "nightly"

src/config.rs

@@ -31,6 +31,8 @@ pub struct Config {
     pub use_rk4_for_dynamics_control: bool,
     /// Use RK4 for updating quadrotor dynamics without controls
     pub use_rk4_for_dynamics_update: bool,
+    // Run the simulation in real time mode
+    pub real_time: bool,
 }
 
 #[derive(serde::Deserialize)]

src/main.rs

@@ -1,5 +1,10 @@
+#![feature(thread_sleep_until)]
 use nalgebra::Vector3;
 use peng_quad::*;
+use std::thread;
+use std::time::Duration;
+use std::time::Instant;
+
 /// Main function for the simulation
 fn main() -> Result<(), SimulationError> {
     env_logger::builder()
@@ -80,7 +85,15 @@ fn main() -> Result<(), SimulationError> {
     }
     log::info!("Starting simulation...");
     let mut i = 0;
+    let frame_time = Duration::from_secs_f32(1.0 / config.simulation.simulation_frequency as f32);
+    let mut next_frame = Instant::now();
+    println!("frame_time: {:?}", frame_time);
     loop {
+        // If real-time mode is enabled, sleep until the next frame simulation frame
+        if config.real_time {
+            thread::sleep_until(next_frame);
+            next_frame += frame_time;
+        }
         let time = quad.time_step * i as f32;
         maze.update_obstacles(quad.time_step);
         update_planner(