add yaml configuration system

Cargo.toml

@@ -25,3 +25,6 @@ rand_distr = "0.4.3"
 rerun = "0.18.0"
 ndarray = "0.15.6"
 thiserror = "1.0.63"
+serde = { version = "1.0.209", features = ["derive"] }
+serde_yaml = "0.9.34"
+lazy_static = "1.5.0"

README.md

@@ -93,12 +93,23 @@ I would like to thank [tokei](https://github.com/XAMPPRocky/tokei) for providing
 My goal is to keep the project minimal and easy to understand by keeping the code line count below 1500.
 The markdown lines are used for generating the documentation.
 ```markdown
-========================================================
- Language     Files    Lines     Code Comments   Blanks
-========================================================
- Rust             1     1270     1233       12       25
- |- Markdown      1      291        0      291        0
- (Total)                1561     1233      303       25
+===============================================================================
+Language            Files        Lines         Code     Comments       Blanks
+===============================================================================
+SVG                     1            1            1            0            0
+TOML                    1           30           29            0            1
+YAML                    1           93           86            0            7
+-------------------------------------------------------------------------------
+Markdown                4          274            0          210           64
+|- Markdown             1           17            0           17            0
+(Total)                            291            0          227           64
+-------------------------------------------------------------------------------
+Rust                    3         1542         1491           13           38
+|- Markdown             2          301            0          301            0
+(Total)                           1843         1491          314           38
+===============================================================================
+Total                  10         1940         1607          223          110
+===============================================================================
  ```
 
 ## Star History

config/quad.yaml

@@ -0,0 +1,93 @@
+simulation:
+  control_frequency: 200.0
+  simulation_frequency: 1000.0
+  log_frequency: 20.0
+  duration: 70.0 # in seconds
+
+quadrotor:
+  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]
+
+controller:
+  pos_gains:
+    kp: [7.1, 7.1, 11.9]
+    kd: [2.4, 2.4, 6.7]
+    ki: [0.0, 0.0, 0.0]
+  att_gains:
+    kp: [1.5, 1.5, 1.0]
+    kd: [0.13, 0.13, 0.1]
+    ki: [0.0, 0.0, 0.0]
+  pos_max_int: [10.0, 10.0, 10.0]
+  att_max_int: [0.5, 0.5, 0.5]
+
+imu:
+  accel_noise_std: 0.02
+  gyro_noise_std: 0.01
+  bias_instability: 0.0001
+
+maze:
+  lower_bounds: [-3.0, -2.0, 0.0]
+  upper_bounds: [3.0, 2.0, 2.0]
+  num_obstacles: 20
+
+camera:
+  resolution: [128, 96]
+  fov: 90.0
+  near: 0.1
+  far: 5.0
+
+mesh:
+  division: 7
+  spacing: 0.5
+
+planner_schedule:
+  steps:
+    - step: 1000
+      planner_type: MinimumJerkLine
+      params:
+        end_position: [0.0, 0.0, 1.0]
+        end_yaw: 0.0
+        duration: 2.5
+    - step: 5000
+      planner_type: Lissajous
+      params:
+        center: [0.5, 0.5, 1.0]
+        amplitude: [0.5, 0.5, 0.2]
+        frequency: [1.0, 2.0, 3.0]
+        phase: [0.0, 1.5707963267948966, 0.0] # [0.0, PI/2, 0.0]
+        duration: 20.0
+        end_yaw: 6.283185307179586 # 2*PI
+        ramp_time: 5.0
+    - step: 27000
+      planner_type: Circle
+      params:
+        center: [0.5, 0.5, 1.0]
+        radius: 0.5
+        angular_velocity: 1.0
+        duration: 8.0
+        ramp_time: 2.0
+    - step: 37000
+      planner_type: ObstacleAvoidance
+      params:
+        target_position: [1.5, 1.0, 0.5]
+        duration: 10.0
+        end_yaw: 0.0
+        k_att: 0.03
+        k_rep: 0.02
+        d0: 0.5
+    - step: 45000
+      planner_type: MinimumSnapWaypoint
+      params:
+        waypoints:
+          - [1.0, 1.0, 1.5]
+          - [-1.0, 1.0, 1.75]
+          - [0.0, -1.0, 1.0]
+          - [0.0, 0.0, 0.5]
+        yaws: [1.5707963267948966, 3.141592653589793, -1.5707963267948966, 0.0] # [PI/2, PI, -PI/2, 0.0]
+        segment_times: [5.0, 5.0, 5.0, 5.0]
+    - step: 65000
+      planner_type: Landing
+      params:
+        duration: 5.0

src/config.rs

@@ -0,0 +1,93 @@
+use serde::Deserialize;
+use serde_yaml::Value;
+use std::fs::File;
+use std::io::Read;
+
+#[derive(Deserialize)]
+pub struct Config {
+    pub simulation: SimulationConfig,
+    pub quadrotor: QuadrotorConfig,
+    pub controller: ControllerConfig,
+    pub imu: ImuConfig,
+    pub maze: MazeConfig,
+    pub camera: CameraConfig,
+    pub mesh: MeshConfig,
+    pub planner_schedule: PlannerScheduleConfig,
+}
+
+#[derive(Deserialize)]
+pub struct PlannerScheduleConfig {
+    pub steps: Vec<PlannerStep>,
+}
+
+#[derive(Deserialize)]
+pub struct PlannerStep {
+    pub step: usize,
+    pub planner_type: String,
+    pub params: Value,
+}
+#[derive(Deserialize)]
+pub struct SimulationConfig {
+    pub control_frequency: f32,
+    pub simulation_frequency: f32,
+    pub log_frequency: f32,
+    pub duration: f32,
+}
+
+#[derive(Deserialize)]
+pub struct QuadrotorConfig {
+    pub mass: f32,
+    pub gravity: f32,
+    pub drag_coefficient: f32,
+    pub inertia_matrix: [f32; 9],
+}
+
+#[derive(Deserialize)]
+pub struct ControllerConfig {
+    pub pos_gains: PIDGains,
+    pub att_gains: PIDGains,
+    pub pos_max_int: [f32; 3],
+    pub att_max_int: [f32; 3],
+}
+
+#[derive(Deserialize)]
+pub struct PIDGains {
+    pub kp: [f32; 3],
+    pub ki: [f32; 3],
+    pub kd: [f32; 3],
+}
+
+#[derive(Deserialize, Default)]
+pub struct ImuConfig {
+    pub accel_noise_std: f32,
+    pub gyro_noise_std: f32,
+    pub bias_instability: f32,
+}
+
+#[derive(Deserialize)]
+pub struct MazeConfig {
+    pub upper_bounds: [f32; 3],
+    pub lower_bounds: [f32; 3],
+    pub num_obstacles: usize,
+}
+
+#[derive(Deserialize)]
+pub struct CameraConfig {
+    pub resolution: (usize, usize),
+    pub fov: f32,
+    pub near: f32,
+    pub far: f32,
+}
+
+#[derive(Deserialize)]
+pub struct MeshConfig {
+    pub division: usize,
+    pub spacing: f32,
+}
+impl Config {
+    pub fn from_yaml(filename: &str) -> Result<Self, Box<dyn std::error::Error>> {
+        let mut contents = String::new();
+        File::open(filename)?.read_to_string(&mut contents)?;
+        Ok(serde_yaml::from_str(&contents)?)
+    }
+}