From f8a42ee96ce9b6a45384366b7a551ee8e3731a3c Mon Sep 17 00:00:00 2001
From: Ryan Rodriguez <ryan.and.rodriguez@gmail.com>
Date: Mon, 11 Nov 2024 15:25:14 -0800
Subject: [PATCH] add thrust and arm length values to config

---
 src/config.rs | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/src/config.rs b/src/config.rs
index 8849081d..8082e74d 100644
--- a/src/config.rs
+++ b/src/config.rs
@@ -104,6 +104,26 @@ pub struct QuadrotorConfig {
     pub drag_coefficient: 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,
+}
+
+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,
+            arm_length_m: 0.150,
+            yaw_torque_constant: 0.05,
+        }
+    }
 }
 
 impl QuadrotorConfig {
@@ -119,6 +139,14 @@ impl QuadrotorConfig {
                 "Failed to invert inertia matrix".to_string(),
             ))?)
     }
+
+    /// 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;
+        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)]