From c1041f901e967f5abb00ce8d19974e5caa9a8fa3 Mon Sep 17 00:00:00 2001 From: franchioping Date: Thu, 2 Apr 2026 17:40:15 +0100 Subject: [PATCH] velocity layer working --- analyze.py | 10 ++-- configurations/pid_cont/rate_030.toml | 14 ++++- configurations/pid_cont/rate_031.toml | 14 ++++- configurations/pid_cont/rate_0312.toml | 14 ++++- inputs/vel_all.json | 77 ++++++++++++++++++++++++++ simulation/src/drone.rs | 1 + simulation/src/logger.rs | 1 + simulation/src/simulation.rs | 19 +++++-- 8 files changed, 139 insertions(+), 11 deletions(-) create mode 100644 inputs/vel_all.json diff --git a/analyze.py b/analyze.py index d49537c..17bc860 100644 --- a/analyze.py +++ b/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_current", "Rot Current"), - ("angvel_current", "Angvel Current"), - ("angvel_target", "Angvel Target"), + # ("rot_target", "Rot Target"), + # ("rot_current", "Rot Current"), + # ("angvel_current", "Angvel Current"), + # ("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: diff --git a/configurations/pid_cont/rate_030.toml b/configurations/pid_cont/rate_030.toml index 5af548b..7c19703 100644 --- a/configurations/pid_cont/rate_030.toml +++ b/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 } } diff --git a/configurations/pid_cont/rate_031.toml b/configurations/pid_cont/rate_031.toml index ebfa70e..4b9bca7 100644 --- a/configurations/pid_cont/rate_031.toml +++ b/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 } } diff --git a/configurations/pid_cont/rate_0312.toml b/configurations/pid_cont/rate_0312.toml index a7596bc..ccfa016 100644 --- a/configurations/pid_cont/rate_0312.toml +++ b/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 } } diff --git a/inputs/vel_all.json b/inputs/vel_all.json new file mode 100644 index 0000000..8226c20 --- /dev/null +++ b/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 + } + ] +} diff --git a/simulation/src/drone.rs b/simulation/src/drone.rs index 4a666c6..7b1011d 100644 --- a/simulation/src/drone.rs +++ b/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(); } diff --git a/simulation/src/logger.rs b/simulation/src/logger.rs index 56641d9..be6f1f2 100644 --- a/simulation/src/logger.rs +++ b/simulation/src/logger.rs @@ -52,6 +52,7 @@ impl From for na::Vector3 { 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, diff --git a/simulation/src/simulation.rs b/simulation/src/simulation.rs index e743400..64fe778 100644 --- a/simulation/src/simulation.rs +++ b/simulation/src/simulation.rs @@ -200,10 +200,20 @@ impl Simulation { ] * 3.14 }; - let accel_target = na::vector![ - current_input.acceleration.x, - current_input.acceleration.y, - current_input.acceleration.z + 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, + ] * 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(),