before split into lib

analyze.py

@@ -106,6 +106,9 @@ class SimVisualizer:
                 # Check if the columns are nested and flatten them if necessary
                 # If read_json doesn't flatten automatically, we expand the dicts:
                 for col in [
+                    "linvel_current",
+                    "accel_current",
+                    "accel_target",
                     "angvel_target",
                     "angvel_current",
                     "mot_current",
@@ -132,34 +135,30 @@ class SimVisualizer:
         c = self.axis_var.get()  # current axis (x, y, or z)
         self.ax.clear()
 
-        # Mapping new nested keys to the plot
+        to_plot = [
-        # The keys follow the pattern: category.axis
+            ("rot_target", "Rot Target"),
-        self.ax.plot(
+            ("rot_current", "Rot Current"),
-            self.df["time"], self.df[f"rot_target.{c}"], label=f"Rot Target {c}"
+            ("angvel_current", "Angvel Current"),
-        )
+            ("angvel_target", "Angvel Target"),
-        self.ax.plot(
+            ("accel_target", "Accel Target"),
-            self.df["time"], self.df[f"rot_current.{c}"], label=f"Rot Current {c}"
+            ("accel_current", "Accel Current"),
-        )
+            ("linvel_current", "Linvel Current"),
+        ]
 
-        self.ax.plot(
+        for name, label in to_plot:
-            self.df["time"], self.df[f"angvel_target.{c}"], label=f"AngVel Target {c}"
+            self.ax.plot(self.df["time"], self.df[f"{name}.{c}"], label=f"{label} {c}")
-        )
-        self.ax.plot(
-            self.df["time"], self.df[f"angvel_current.{c}"], label=f"AngVel Current {c}"
-        )
 
-        self.ax.plot(
+        # self.ax.plot(
-            self.df["time"],
+        #     self.df["time"],
-            self.df[f"mot_target.{c}"],
+        #     self.df[f"mot_target.{c}"],
-            label=f"Mot Target {c}",
+        #     label=f"Mot Target {c}",
-            linestyle="--",
+        #     linestyle="--",
-        )
+        # )
-        self.ax.plot(
+        # self.ax.plot(
-            self.df["time"],
+        #     self.df["time"],
-            self.df[f"mot_current.{c}"],
+        #     self.df[f"mot_current.{c}"],
-            label=f"Mot Current {c}",
+        #     label=f"Mot Current {c}",
-            alpha=0.7,
+        # )
-        )
 
         self.ax.set_xlabel("Time (s)")
         self.ax.set_ylabel("Value")

configurations/pid_cont/rate_025.toml

@@ -1,26 +0,0 @@
-
-stack = { max_rate = 3.14, rotation_pid = { kp = [
-    0.7,
-    0.7,
-    0.7,
-], ki = [
-    0.0,
-    0.0,
-    0.0,
-], kd = [
-    0.0,
-    0.0,
-    0.0,
-], frequency = 50.0 }, rate_pid = { kp = [
-    0.1,
-    0.1,
-    1.0,
-], ki = [
-    0.0,
-    0.0,
-    0.0,
-], kd = [
-    0.0,
-    0.0,
-    0.0,
-], frequency = 600.0 } }

configurations/pid_cont/rate_030.toml

@@ -1,20 +1,20 @@
 
 stack = { max_rate = 3.14, rotation_pid = { kp = [
-    1.5,
+    6.0,
-    1.5,
+    6.0,
-    1.5,
+    6.0,
 ], ki = [
-    0.0,
+    2.0,
-    0.0,
+    2.0,
-    0.0,
+    2.0,
 ], kd = [
     0.0,
     0.0,
     0.0,
 ], frequency = 50.0 }, rate_pid = { kp = [
-    0.2,
+    0.1,
-    0.2,
+    0.1,
-    2.0,
+    1.0,
 ], ki = [
     0.0,
     0.0,

configurations/sim/sim_std.toml

@@ -1,2 +1,2 @@
-tickrate = 6000.0
+tickrate = 1200.0
 drone_tick_rate = 600

src/drone.rs

@@ -11,7 +11,7 @@ pub use input::JoystickInput;
 
 const AIR_DENSITY: f32 = 1.23;
 const DRAG_CONSTANT: f32 = 1.3;
-const DRAG_MAGIC_NUM: f32 = 0.25;
+const DRAG_MAGIC_NUM: f32 = 0.00;
 
 pub struct MotorCharacteristics {
     pub relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4],
@@ -55,6 +55,8 @@ pub struct Drone {
     pub current_throttles: [f32; 4],
     target_throttles: [f32; 4],
     last_time: f32,
+    linvel: na::Vector3<f32>,
+    accel: na::Vector3<f32>,
 }
 
 fn calculate_drag(velocity: f32, area: f32, constant: f32) -> f32 {
@@ -110,6 +112,8 @@ impl Drone {
             current_throttles: [0.0; 4],
             target_throttles: [0.0; 4],
             last_time: world.get_time(),
+            linvel: na::Vector3::zeros(),
+            accel: na::Vector3::zeros(),
         };
     }
 
@@ -138,7 +142,6 @@ impl Drone {
          */
         drone_rb.reset_forces(true);
         drone_rb.reset_torques(true);
-
         for (i, motor_position) in self
             .motor_characteristics
             .relative_motor_positions
@@ -191,9 +194,17 @@ impl Drone {
         self.target_throttles = self.controller.get_motor_throttles();
     }
 
+    pub fn get_accel(&self) -> na::Vector3<f32> {
+        self.accel
+    }
+
     pub fn get_angvel(&self, world: &World) -> na::Vector3<f32> {
         *world.bodies.get(self.rb_handle).unwrap().angvel()
     }
+
+    pub fn get_rb<'a>(&self, world: &'a World) -> &'a rp::RigidBody {
+        world.bodies.get(self.rb_handle).unwrap()
+    }
     pub fn get_rot(&self, world: &World) -> na::Unit<na::Quaternion<f32>> {
         *world.bodies.get(self.rb_handle).unwrap().rotation()
     }
@@ -205,6 +216,10 @@ impl Drone {
     pub fn process_tick(&mut self, world: &mut World) {
         self.apply_throttles(world, world.get_time() - self.last_time);
         self.apply_drag(world);
+
+        let cur_linvel = self.get_rb(world).linvel();
+        self.accel = (cur_linvel - self.linvel) / (world.get_time() - self.last_time);
+        self.linvel = *cur_linvel;
         self.last_time = world.get_time();
     }
 }

src/drone/stacked.rs

@@ -18,11 +18,13 @@ use modules::*;
 pub struct DroneState {
     pub rotation: na::UnitQuaternion<f32>,
     pub angular_velocity: na::Vector3<f32>,
+    pub mass: f32,
 }
 
 pub struct StackedController {
     pub rotation_rt: ModuleRuntime<RotationController>,
     pub rate_rt: ModuleRuntime<AngularRateController>,
+    pub linaccel_rt: ModuleRuntime<AccelerationController>,
 
     mixer: MotorMixer,
     config: SimulationConfig,
@@ -36,23 +38,30 @@ pub struct StackedController {
 
 impl StackedController {
     pub fn new(config: SimulationConfig) -> Self {
+        let min_throttle = 0.0;
+        let max_throttle = 1.0;
+
+        let lin_accel_ctrl =
+            AccelerationController::new(max_throttle, min_throttle, config.max_thrust);
         let rotation_ctrl = RotationController::new(PidProcessor::new(&config.stack.rotation_pid));
         let rate_ctrl = AngularRateController::new(PidProcessor::new(&config.stack.rate_pid));
 
         Self {
+            linaccel_rt: ModuleRuntime::new(lin_accel_ctrl, config.stack.rotation_pid.frequency),
             rotation_rt: ModuleRuntime::new(rotation_ctrl, config.stack.rotation_pid.frequency),
             rate_rt: ModuleRuntime::new(rate_ctrl, config.stack.rate_pid.frequency),
             mixer: MotorMixer {
                 motor_map: config.motor_map,
-                min_throttle: 0.1,
+                min_throttle: 0.0,
                 max_throttle: 1.0,
-                mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 },
+                mixing_mode: mixer::MotorMixingMode::default(),
             },
-            config,
+            config: config.clone(),
             input: Input::default(),
             drone_state: DroneState {
                 rotation: na::UnitQuaternion::identity(),
                 angular_velocity: na::Vector3::zeros(),
+                mass: config.mass,
             },
             last_time: 0.0,
             current_time: 0.0,
@@ -78,10 +87,24 @@ impl DroneController for StackedController {
 
     fn get_motor_throttles(&mut self) -> [f32; 4] {
         let frame_dt = (self.current_time - self.last_time).max(0.0);
+        let mut throttle = self.input.joystick.throttle_input;
 
         let angular_rate_setpoint = if self.input.mode != ModeInput::Acro {
             let rotation_setpoint = if self.input.mode != ModeInput::Rotation {
+                let lin_accel_setpoint = if self.input.mode != ModeInput::Acceleration {
                     panic!("Not Implemented")
+                } else {
+                    Acceleration(na::vector![
+                        self.input.acceleration.x,
+                        self.input.acceleration.y,
+                        self.input.acceleration.z
+                    ])
+                };
+                let ret = self
+                    .linaccel_rt
+                    .update(lin_accel_setpoint, &self.drone_state, frame_dt);
+                throttle = ret.1 .0;
+                ret.0
             } else {
                 // println!("Rotation!");
                 Rotation(na::vector![
@@ -106,7 +129,7 @@ impl DroneController for StackedController {
             .rate_rt
             .update(angular_rate_setpoint, &self.drone_state, frame_dt);
 
-        self.mixer.mix(self.input.joystick.throttle_input, torque.0)
+        self.mixer.mix(throttle, torque.0)
     }
 
     fn as_any(&self) -> &dyn Any {

src/drone/stacked/mixer.rs

@@ -82,9 +82,9 @@ impl MotorMixer {
         let delta_dif = max_delta - min_delta;
 
         if delta_dif > throttle_range_len {
-            scale *= throttle_range_len / delta_dif * 0.9;
+            scale *= throttle_range_len / delta_dif * 0.99;
-            max_delta *= throttle_range_len / delta_dif * 0.9;
+            max_delta *= throttle_range_len / delta_dif * 0.99;
-            min_delta *= throttle_range_len / delta_dif * 0.9;
+            min_delta *= throttle_range_len / delta_dif * 0.99;
         }
 
         let lim_throttle = throttle.clamp(

src/logger.rs

@@ -51,6 +51,9 @@ impl From<Vec3Serialize> for na::Vector3<f32> {
 #[derive(Debug, Serialize)]
 pub struct SimLogRow {
     pub time: f32,
+    pub linvel_current: Vec3Serialize,
+    pub accel_current: Vec3Serialize,
+    pub accel_target: Vec3Serialize,
     pub angvel_target: Vec3Serialize,
     pub angvel_current: Vec3Serialize,
     pub mot_current: Vec3Serialize,

src/simulation.rs

@@ -159,33 +159,8 @@ impl Simulation {
         }
         self.drone.process_tick(&mut self.world);
 
-        let mut target_angular_vel: na::Vector3<f32> = na::vector![
+        /* Logging */
-            current_input.joystick.roll_input,
-            current_input.joystick.pitch_input,
-            current_input.joystick.yaw_input,
-        ] * 3.14;
 
-        let mut target_mot: na::Vector3<f32> = Default::default();
-        if let Some(cont) = self
-            .drone
-            .controller
-            .as_mut_any()
-            .downcast_mut::<crate::drone::stacked::StackedController>()
-        {
-            if current_input.mode != ModeInput::Acro {
-                target_angular_vel = cont.rotation_rt.last_output.unwrap_or_default().0;
-            }
-            target_mot = cont.rate_rt.last_output.unwrap_or_default().0;
-        }
-        /*
-        # roll, pitch, yaw
-        motor_map = [
-            [1.0, -1.0, 1.0],   # Front Right
-            [-1.0, -1.0, -1.0], # Front Left
-            [-1.0, 1.0, 1.0],   # Rear Left
-            [1.0, 1.0, -1.0],   # Rear Right
-        ]
-        */
         let applied_motor_diff: na::Vector3<f32> = na::vector![
             (self.drone.current_throttles[0] + self.drone.current_throttles[3]
                 - self.drone.current_throttles[1]
@@ -198,10 +173,49 @@ impl Simulation {
                 - self.drone.current_throttles[3])
         ] / 2.0;
 
+        if let Some(cont) = self
+            .drone
+            .controller
+            .as_mut_any()
+            .downcast_mut::<crate::drone::stacked::StackedController>()
+        {
+            let target_mot = cont.rate_rt.last_output.unwrap_or_default().0;
+
+            let target_angular_vel = if current_input.mode != ModeInput::Acro {
+                cont.rotation_rt.last_output.unwrap_or_default().0
+            } else {
+                na::vector![
+                    current_input.joystick.roll_input,
+                    current_input.joystick.pitch_input,
+                    current_input.joystick.yaw_input,
+                ] * 3.14
+            };
+
+            let target_rot = if current_input.mode != ModeInput::Rotation {
+                cont.linaccel_rt.last_output.unwrap_or_default().0 .0
+            } else {
+                na::vector![
+                    current_input.rotation.roll,
+                    current_input.rotation.pitch,
+                    current_input.rotation.yaw,
+                ] * 3.14
+            };
+
+            let accel_target = na::vector![
+                current_input.acceleration.x,
+                current_input.acceleration.y,
+                current_input.acceleration.z
+            ];
+
+            let accel = self.drone.get_accel();
+
             if let Some(logger) = &mut self.logger {
                 logger
                     .log(&SimLogRow {
                         time: self.world.get_time(),
+                        linvel_current: (*self.drone.get_rb(&self.world).linvel()).into(),
+                        accel_current: accel.into(),
+                        accel_target: accel_target.into(),
                         angvel_target: target_angular_vel.into(),
                         angvel_current: self
                             .drone
@@ -210,11 +224,7 @@ impl Simulation {
                             .transform_vector(&self.drone.get_angvel(&self.world))
                             .into(),
                         rot_current: self.drone.get_rot(&self.world).scaled_axis().into(),
-                    rot_target: na::UnitQuaternion::from_scaled_axis(vector![
+                        rot_target: na::UnitQuaternion::from_scaled_axis(target_rot)
-                        current_input.rotation.roll,
-                        current_input.rotation.pitch,
-                        current_input.rotation.yaw
-                    ])
                             .scaled_axis()
                             .into(),
                         mot_current: applied_motor_diff.into(),
@@ -222,6 +232,8 @@ impl Simulation {
                     })
                     .unwrap();
             }
+        }
+
         Ok(StepOutcome::Continue)
     }