velocity layer working

analyze.py

@@ -107,6 +107,7 @@ class SimVisualizer:
                 # If read_json doesn't flatten automatically, we expand the dicts:
                 for col in [
                     "linvel_current",
+                    "linvel_target",
                     "accel_current",
                     "accel_target",
                     "angvel_target",
@@ -136,13 +137,14 @@ class SimVisualizer:
         self.ax.clear()
 
         to_plot = [
-            ("rot_target", "Rot Target"),
+            # ("rot_target", "Rot Target"),
-            ("rot_current", "Rot Current"),
+            # ("rot_current", "Rot Current"),
-            ("angvel_current", "Angvel Current"),
+            # ("angvel_current", "Angvel Current"),
-            ("angvel_target", "Angvel Target"),
+            # ("angvel_target", "Angvel Target"),
             ("accel_target", "Accel Target"),
             ("accel_current", "Accel Current"),
             ("linvel_current", "Linvel Current"),
+            ("linvel_target", "Linvel Target"),
         ]
 
         for name, label in to_plot:

configurations/pid_cont/rate_030.toml

@@ -23,4 +23,16 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
     0.0,
     0.0,
     0.0,
-], frequency = 600.0 } }
+], frequency = 600.0 }, linvel_pid = { kp = [
+    4.0,
+    4.0,
+    4.0,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 25.0 } }

configurations/pid_cont/rate_031.toml

@@ -23,4 +23,16 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
     0.0,
     0.0,
     0.0,
-], frequency = 600.0 } }
+], frequency = 600.0 }, linvel_pid = { kp = [
+    4.0,
+    4.0,
+    4.0,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 25.0 } }

configurations/pid_cont/rate_0312.toml

@@ -23,4 +23,16 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [
     0.0,
     0.0,
     0.0,
-], frequency = 600.0 } }
+], frequency = 600.0 }, linvel_pid = { kp = [
+    4.0,
+    4.0,
+    4.0,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 25.0 } }

inputs/vel_all.json

@@ -0,0 +1,77 @@
+{
+  "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
+        },
+        "velocity": {
+          "x": 0.15,
+          "y": 0.15,
+          "z": 0
+        },
+        "mode": "Velocity"
+      },
+      "time": 0
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "velocity": {
+          "x": 0.3,
+          "y": -1.3,
+          "z": 0
+        },
+        "mode": "Velocity"
+      },
+      "time": 3
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "velocity": {
+          "x": 3.4,
+          "y": 0,
+          "z": 1
+        },
+        "mode": "Velocity"
+      },
+      "time": 10
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "mode": "Velocity"
+      },
+      "time": 20
+    }
+  ]
+}

simulation/src/drone.rs

@@ -154,6 +154,7 @@ impl Drone {
         self.controller
             .set_angular_velocity(rb.rotation().inverse().transform_vector(&rb.angvel()));
         self.controller.set_rotation(*rb.rotation());
+        self.controller.set_linvel(self.linvel);
         self.target_throttles = self.controller.get_motor_throttles();
     }
 

simulation/src/logger.rs

@@ -52,6 +52,7 @@ impl From<Vec3Serialize> for na::Vector3<f32> {
 pub struct SimLogRow {
     pub time: f32,
     pub linvel_current: Vec3Serialize,
+    pub linvel_target: Vec3Serialize,
     pub accel_current: Vec3Serialize,
     pub accel_target: Vec3Serialize,
     pub angvel_target: Vec3Serialize,

simulation/src/simulation.rs

@@ -200,10 +200,20 @@ impl Simulation {
                 ] * 3.14
             };
 
-            let accel_target = na::vector![
+            let accel_target = if current_input.mode != ModeInput::Acceleration {
+                cont.linvel_rt.last_output.unwrap_or_default().0
+            } else {
+                na::vector![
                     current_input.acceleration.x,
                     current_input.acceleration.y,
-                current_input.acceleration.z
+                    current_input.acceleration.z,
+                ] * 3.14
+            };
+
+            let linvel_target = na::vector![
+                current_input.velocity.x,
+                current_input.velocity.y,
+                current_input.velocity.z
             ];
 
             let accel = self.drone.get_accel();
@@ -213,6 +223,7 @@ impl Simulation {
                     .log(&SimLogRow {
                         time: self.world.get_time(),
                         linvel_current: (*self.drone.get_rb(&self.world).linvel()).into(),
+                        linvel_target: linvel_target.into(),
                         accel_current: accel.into(),
                         accel_target: accel_target.into(),
                         angvel_target: target_angular_vel.into(),