changed the vertical axis from y to z

configurations/gen.py

@@ -1,11 +1,14 @@
 import os
 import subprocess
 
-subprocess.run("rm -rf final && mkdir final", shell=True)
+FOLDER_PREFIX = "configurations/"
+MOT_FOLDER = FOLDER_PREFIX + "mot/"
+SIM_FOLDER = FOLDER_PREFIX + "sim/"
+PID_FOLDER = FOLDER_PREFIX + "pid_cont/"
+FINAL_FOLDER = FOLDER_PREFIX + "final/"
+
+subprocess.run(f"rm -rf {FINAL_FOLDER} && mkdir {FINAL_FOLDER}", shell=True)
 
-MOT_FOLDER = "mot/"
-SIM_FOLDER = "sim/"
-PID_FOLDER = "pid_cont/"
 
 mot_files = os.listdir(MOT_FOLDER)
 sim_files = os.listdir(SIM_FOLDER)
@@ -22,7 +25,7 @@ for case in prod:
             with open(PID_FOLDER + case[2], "r") as f3:
                 settings = f1.read() + "\n" + f2.read() + "\n" + f3.read()
     with open(
-        "final/" + "".join(case).replace(".toml", "_").removesuffix("_") + ".toml",
+        FINAL_FOLDER + "".join(case).replace(".toml", "_").removesuffix("_") + ".toml",
         "w",
     ) as f:
         f.write(settings)

configurations/mot/mot_std.toml

@@ -5,9 +5,10 @@ mass = 0.350
 
 time_constant = 0.00
 
+# roll, pitch, yaw
 motor_map = [
-    [-1.0, 1.0, 1.0],   # Front Right
+    [1.0, -1.0, 1.0],   # Front Right
-    [1.0, -1.0, 1.0],   # Front Left
+    [-1.0, -1.0, -1.0], # Front Left
-    [1.0, 1.0, -1.0],   # Rear Left
+    [-1.0, 1.0, 1.0],   # Rear Left
-    [-1.0, -1.0, -1.0], # Rear Right
+    [1.0, 1.0, -1.0],   # Rear Right
 ]

configurations/mot/mot_tc.toml

@@ -1,12 +0,0 @@
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-
-time_constant = 0.01
-
-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
-]

configurations/pid_cont/rate_025.toml

@@ -12,9 +12,9 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
     0.0,
     0.0,
 ], frequency = 50.0 }, rate_pid = { kp = [
-    0.03,
+    0.1,
-    0.3,
+    0.1,
-    0.03,
+    1.0,
 ], ki = [
     0.0,
     0.0,

configurations/pid_cont/rate_030.toml

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

configurations/pid_cont_old/rate_03_x.toml

@@ -13,8 +13,8 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
     0.0,
 ], frequency = 50.0 }, rate_pid = { kp = [
     0.03,
-    0.3,
+    0.0,
-    0.03,
+    0.0,
 ], ki = [
     0.0,
     0.0,

configurations/pid_cont_old/rate_03_y.toml

@@ -1,8 +1,8 @@
 
 stack = { max_rate = 3.14, rotation_pid = { kp = [
-    0.5,
+    1.0,
-    0.5,
+    1.0,
-    0.5,
+    1.0,
 ], ki = [
     0.0,
     0.0,
@@ -11,10 +11,10 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
     0.0,
     0.0,
     0.0,
-], frequency = 100.0 }, rate_pid = { kp = [
+], frequency = 50.0 }, rate_pid = { kp = [
-    0.03,
+    0.0,
-    0.3,
     0.03,
+    0.0,
 ], ki = [
     0.0,
     0.0,

configurations/pid_cont_old/rate_03_z.toml

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

inputs/not/rot_all.json

@@ -0,0 +1,92 @@
+{
+  "records": [
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": 0,
+          "yaw": 0,
+          "pitch": 0
+        },
+        "mode": "Rotation"
+      },
+      "time": 0
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": 3.14,
+          "yaw": 3.1,
+          "pitch": 1.5
+        },
+        "mode": "Rotation"
+      },
+      "time": 3
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": -1.5,
+          "yaw": 0.8,
+          "pitch": 0.4
+        },
+        "mode": "Rotation"
+      },
+      "time": 10
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": 1.5,
+          "yaw": 0.1,
+          "pitch": 2
+        },
+        "mode": "Rotation"
+      },
+      "time": 20
+    }
+  ]
+}

src/config.rs

@@ -2,7 +2,7 @@ use nalgebra::{vector, Vector3};
 use rapier3d::prelude::*;
 use serde::Deserialize;
 
-#[derive(Debug, Deserialize, Clone)]
+#[derive(Default, Debug, Deserialize, Clone)]
 pub struct PidConfig {
     pub kp: [f32; 3],
     pub ki: [f32; 3],
@@ -26,8 +26,10 @@ impl PidConfig {
 #[derive(Debug, Deserialize, Clone)]
 pub struct ControllerStackConfig {
     /// PID for the rotation (angle → angular rate) layer
+    #[serde(default)]
     pub rotation_pid: PidConfig,
 
+    #[serde(default)]
     pub acceleration_pid: PidConfig,
 
     /// PID for the angular rate (angular rate → torque) layer

src/drone.rs

@@ -34,10 +34,10 @@ impl Default for MotorCharacteristics {
              * 2 --- 3
              */
             relative_motor_positions: [
-                rp::point![5.0, 0.0, 5.0],
+                rp::point![5.0, 5.0, 0.0],
-                rp::point![5.0, 0.0, -5.0],
+                rp::point![5.0, -5.0, 0.0],
-                rp::point![-5.0, 0.0, -5.0],
+                rp::point![-5.0, -5.0, 0.0],
-                rp::point![-5.0, 0.0, 5.0],
+                rp::point![-5.0, 5.0, 0.0],
             ],
             max_thrust: 2.6,
             max_torque: 0.5,
@@ -70,7 +70,7 @@ impl Drone {
     ) -> Drone {
         let drone_rb_handle = world.register_body(
             rp::RigidBodyBuilder::dynamic()
-                .translation(rp::vector![0.0, 10.0, 0.0])
+                .translation(rp::vector![0.0, 0.0, 10.0])
                 .rotation(rp::vector![0.0, 0.0, 0.0])
                 /*
                  * These damping values keep the simulation more realistic,
@@ -164,7 +164,7 @@ impl Drone {
             // Apply RB's rotation to point at RB's Up
             //
             // motor_position is relative, transform it by the RB's position first
-            let mut thrust_force = nalgebra::Vector3::new(0.0, thrust, 0.0);
+            let mut thrust_force = nalgebra::Vector3::new(0.0, 0.0, thrust);
             thrust_force = drone_rb.rotation().transform_vector(&thrust_force);
             drone_rb.add_force_at_point(
                 thrust_force,
@@ -173,9 +173,9 @@ impl Drone {
             );
 
             let torque = if i % 2 == 0 {
-                rp::vector![0.0, torque, 0.0]
+                rp::vector![0.0, 0.0, torque]
             } else {
-                rp::vector![0.0, -torque, 0.0]
+                rp::vector![0.0, 0.0, -torque]
             };
             drone_rb.add_torque(drone_rb.rotation().transform_vector(&torque), true);
         }

src/drone/input.rs

@@ -44,9 +44,13 @@ pub enum ModeInput {
 
 #[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
 pub struct Input {
+    #[serde(default)]
     pub joystick: JoystickInput,
+    #[serde(default)]
     pub acceleration: AccelerationInput,
+    #[serde(default)]
     pub rotation: RotationInput,
+    #[serde(default)]
     pub position: PositionInput,
     pub mode: ModeInput,
 }

src/drone/stacked.rs

@@ -86,8 +86,8 @@ impl DroneController for StackedController {
                 // println!("Rotation!");
                 Rotation(na::vector![
                     self.input.rotation.roll,
-                    self.input.rotation.yaw,
                     self.input.rotation.pitch,
+                    self.input.rotation.yaw,
                 ])
             };
             self.rotation_rt
@@ -96,8 +96,8 @@ impl DroneController for StackedController {
             AngularRate(
                 na::vector![
                     self.input.joystick.roll_input,
-                    self.input.joystick.yaw_input,
                     self.input.joystick.pitch_input,
+                    self.input.joystick.yaw_input,
                 ] * self.config.stack.max_rate,
             )
         };

src/drone/stacked/mixer.rs

@@ -12,7 +12,7 @@ impl Default for MotorMixingMode {
 }
 
 pub struct MotorMixer {
-    pub motor_map: [[f32; 3]; 4], // roll, yaw, pitch
+    pub motor_map: [[f32; 3]; 4], // roll, pitch, yaw
     pub min_throttle: f32,
     pub max_throttle: f32,
     pub mixing_mode: MotorMixingMode,
@@ -77,12 +77,14 @@ impl MotorMixer {
 
         scale = scale.clamp(min_scale, 1.0);
 
-        let max_delta = scale * delta.into_iter().reduce(f32::max).unwrap_or(0.0);
+        let mut max_delta = scale * delta.into_iter().reduce(f32::max).unwrap_or(0.0);
-        let min_delta = scale * delta.into_iter().reduce(f32::min).unwrap_or(0.0);
+        let mut min_delta = scale * delta.into_iter().reduce(f32::min).unwrap_or(0.0);
         let delta_dif = max_delta - min_delta;
 
         if delta_dif > throttle_range_len {
-            scale *= throttle_range_len / delta_dif;
+            scale *= throttle_range_len / delta_dif * 0.9;
+            max_delta *= throttle_range_len / delta_dif * 0.9;
+            min_delta *= throttle_range_len / delta_dif * 0.9;
         }
 
         let lim_throttle = throttle.clamp(

src/engine.rs

@@ -38,7 +38,7 @@ pub struct ColliderExtraData {
 
 impl World {
     pub fn new(dt: f32) -> Self {
-        let gravity: na::Vector3<f32> = rp::vector![0.0, -9.81, 0.0];
+        let gravity: na::Vector3<f32> = rp::vector![0.0, 0.0, -9.81];
         let mut integration_parameters = rp::IntegrationParameters::default();
         integration_parameters.set_inv_dt(dt);
         let physics_pipeline = rp::PhysicsPipeline::new();
@@ -153,7 +153,7 @@ impl World {
 
 impl Default for World {
     fn default() -> Self {
-        let gravity: na::Vector3<f32> = rp::vector![0.0, -9.81, 0.0];
+        let gravity: na::Vector3<f32> = rp::vector![0.0, 0.0, -9.81];
         let mut integration_parameters = rp::IntegrationParameters::default();
         integration_parameters.set_inv_dt(600.0);
         let physics_pipeline = rp::PhysicsPipeline::new();

src/simulation.rs

@@ -6,7 +6,7 @@ use std::error::Error;
 use crate::{
     drone::{
         controller::DroneController,
-        input::{Input, InputRecording},
+        input::{Input, InputRecording, ModeInput},
         Drone,
     },
     engine::World,
@@ -161,8 +161,8 @@ impl Simulation {
 
         let mut target_angular_vel: na::Vector3<f32> = na::vector![
             current_input.joystick.roll_input,
-            current_input.joystick.yaw_input,
             current_input.joystick.pitch_input,
+            current_input.joystick.yaw_input,
         ] * 3.14;
 
         let mut target_mot: na::Vector3<f32> = Default::default();
@@ -172,19 +172,30 @@ impl Simulation {
             .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[1] + self.drone.current_throttles[2]
+            (self.drone.current_throttles[0] + self.drone.current_throttles[3]
-                - self.drone.current_throttles[0]
-                - self.drone.current_throttles[3]),
-            (self.drone.current_throttles[0] + self.drone.current_throttles[2]
                 - self.drone.current_throttles[1]
-                - self.drone.current_throttles[3]),
+                - self.drone.current_throttles[2]),
             (self.drone.current_throttles[2] + self.drone.current_throttles[3]
                 - self.drone.current_throttles[0]
-                - self.drone.current_throttles[1])
+                - self.drone.current_throttles[1]),
+            (self.drone.current_throttles[0] + self.drone.current_throttles[2]
+                - self.drone.current_throttles[1]
+                - self.drone.current_throttles[3])
         ] / 2.0;
 
         if let Some(logger) = &mut self.logger {
@@ -201,8 +212,8 @@ impl Simulation {
                     rot_current: self.drone.get_rot(&self.world).scaled_axis().into(),
                     rot_target: na::UnitQuaternion::from_scaled_axis(vector![
                         current_input.rotation.roll,
-                        current_input.rotation.yaw,
+                        current_input.rotation.pitch,
-                        current_input.rotation.pitch
+                        current_input.rotation.yaw
                     ])
                     .scaled_axis()
                     .into(),