rotation controller base. added new bin for figuring out quaternions. delete later? add new inputs and modules
This commit is contained in:
franchioping
parent
137a610ab9
commit
6225173006
|
|
@ -34,6 +34,12 @@ version = "0.2.21"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923"
|
||||
|
||||
[[package]]
|
||||
name = "android-tzdata"
|
||||
version = "0.1.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0"
|
||||
|
||||
[[package]]
|
||||
name = "android_system_properties"
|
||||
version = "0.1.5"
|
||||
|
|
@ -180,14 +186,15 @@ checksum = "613afe47fcd5fac7ccf1db93babcb082c5994d996f20b8b159f2ad1658eb5724"
|
|||
|
||||
[[package]]
|
||||
name = "chrono"
|
||||
version = "0.4.43"
|
||||
version = "0.4.39"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "fac4744fb15ae8337dc853fee7fb3f4e48c0fbaa23d0afe49c447b4fab126118"
|
||||
checksum = "7e36cc9d416881d2e24f9a963be5fb1cd90966419ac844274161d10488b3e825"
|
||||
dependencies = [
|
||||
"android-tzdata",
|
||||
"iana-time-zone",
|
||||
"num-traits",
|
||||
"serde",
|
||||
"windows-link",
|
||||
"windows-targets",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
|
|||
|
|
@ -20,7 +20,6 @@ csv = "1.4.0"
|
|||
serde_with = "3"
|
||||
serde_json = "1.0.149"
|
||||
|
||||
|
||||
clap = { version = "4", features = ["derive"] }
|
||||
|
||||
|
||||
|
|
@ -33,3 +32,8 @@ opt-level = 3
|
|||
[[bin]]
|
||||
name = "record"
|
||||
path = "src/main_record.rs"
|
||||
|
||||
|
||||
[[bin]]
|
||||
name = "test"
|
||||
path = "src/main_testing.rs"
|
||||
|
|
|
|||
94
analyze.py
94
analyze.py
|
|
@ -1,3 +1,4 @@
|
|||
import json
|
||||
import tkinter as tk
|
||||
from pathlib import Path
|
||||
from tkinter import messagebox, ttk
|
||||
|
|
@ -10,10 +11,8 @@ from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolb
|
|||
class SimVisualizer:
|
||||
def __init__(self, root: tk.Tk):
|
||||
self.root = root
|
||||
self.root.title("Simulation Data Viewer")
|
||||
self.root.title("Simulation Data Viewer (JSON Format)")
|
||||
|
||||
# --- Protocol Handler ---
|
||||
# This handles the "X" button on the window frame
|
||||
self.root.protocol("WM_DELETE_WINDOW", self.quit_app)
|
||||
|
||||
self.results_dir = Path("results")
|
||||
|
|
@ -24,16 +23,11 @@ class SimVisualizer:
|
|||
self.refresh_file_list()
|
||||
|
||||
def setup_ui(self):
|
||||
# 1. Create a PanedWindow (Horizontal orientation)
|
||||
self.paned_window = ttk.PanedWindow(self.root, orient=tk.HORIZONTAL)
|
||||
self.paned_window.pack(fill=tk.BOTH, expand=True)
|
||||
|
||||
# --- Sidebar Frame ---
|
||||
# Note: We still define a width, but it's now the *initial* width
|
||||
# --- Sidebar ---
|
||||
sidebar = ttk.Frame(self.paned_window, width=300, padding="10")
|
||||
|
||||
# 2. Add the sidebar to the PanedWindow
|
||||
# 'weight=0' keeps it from growing automatically when the window is resized
|
||||
self.paned_window.add(sidebar, weight=0)
|
||||
|
||||
ttk.Label(sidebar, text="Result Files", font=("Helvetica", 12, "bold")).pack(
|
||||
|
|
@ -59,7 +53,6 @@ class SimVisualizer:
|
|||
command=self.update_plot,
|
||||
).pack(anchor=tk.W)
|
||||
|
||||
# --- Quit Button ---
|
||||
spacer = ttk.Label(sidebar, text="")
|
||||
spacer.pack(fill=tk.Y, expand=True)
|
||||
|
||||
|
|
@ -70,9 +63,6 @@ class SimVisualizer:
|
|||
|
||||
# --- Plot Frame ---
|
||||
self.plot_frame = ttk.Frame(self.paned_window, padding="10")
|
||||
|
||||
# 3. Add the plot frame to the PanedWindow
|
||||
# 'weight=1' ensures the plot area expands to take up all remaining space
|
||||
self.paned_window.add(self.plot_frame, weight=1)
|
||||
|
||||
self.fig, self.ax = plt.subplots(figsize=(8, 6))
|
||||
|
|
@ -83,7 +73,6 @@ class SimVisualizer:
|
|||
self.toolbar.update()
|
||||
|
||||
def quit_app(self):
|
||||
"""Cleanly close the plots and the application."""
|
||||
plt.close("all")
|
||||
self.root.destroy()
|
||||
self.root.quit()
|
||||
|
|
@ -92,9 +81,16 @@ class SimVisualizer:
|
|||
if not self.results_dir.exists():
|
||||
self.results_dir.mkdir(parents=True)
|
||||
|
||||
csv_files = sorted(self.results_dir.glob("*.csv"))
|
||||
# Updated to look for .json or .log files
|
||||
log_files = sorted(
|
||||
[
|
||||
f
|
||||
for f in self.results_dir.glob("*")
|
||||
if f.suffix in [".json", ".log", ".txt"]
|
||||
]
|
||||
)
|
||||
self.file_listbox.delete(0, tk.END)
|
||||
for f in csv_files:
|
||||
for f in log_files:
|
||||
self.file_listbox.insert(tk.END, f.name)
|
||||
|
||||
def on_file_select(self, event):
|
||||
|
|
@ -103,42 +99,74 @@ class SimVisualizer:
|
|||
filename = self.file_listbox.get(selection[0])
|
||||
self.current_file = self.results_dir / filename
|
||||
try:
|
||||
self.df = pd.read_csv(self.current_file)
|
||||
# Use Pandas' built-in fast JSON engine
|
||||
# 'lines=True' treats each line as a JSON object
|
||||
self.df = pd.read_json(self.current_file, lines=True)
|
||||
|
||||
# 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 [
|
||||
"angvel_target",
|
||||
"angvel_current",
|
||||
"mot_current",
|
||||
"mot_target",
|
||||
"rot_target",
|
||||
"rot_current",
|
||||
]:
|
||||
if col in self.df.columns:
|
||||
# This expands the dict column into new columns (col.x, col.y, col.z)
|
||||
expanded = pd.json_normalize(self.df[col])
|
||||
expanded.columns = [f"{col}.{c}" for c in expanded.columns]
|
||||
self.df = pd.concat(
|
||||
[self.df.drop(columns=[col]), expanded], axis=1
|
||||
)
|
||||
|
||||
self.update_plot()
|
||||
except Exception as e:
|
||||
messagebox.showerror("Error", f"Could not load file: {e}")
|
||||
messagebox.showerror("Error", f"Could not load JSON file: {e}")
|
||||
|
||||
def update_plot(self):
|
||||
if self.df is None:
|
||||
return
|
||||
|
||||
coord = self.axis_var.get()
|
||||
c = self.axis_var.get() # current axis (x, y, or z)
|
||||
self.ax.clear()
|
||||
|
||||
# Mapping new nested keys to the plot
|
||||
# The keys follow the pattern: category.axis
|
||||
self.ax.plot(
|
||||
self.df["time"], self.df[f"target_{coord}"], label=f"target_{coord}"
|
||||
self.df["time"], self.df[f"rot_target.{c}"], label=f"Rot Target {c}"
|
||||
)
|
||||
self.ax.plot(
|
||||
self.df["time"], self.df[f"current_{coord}"], label=f"current_{coord}"
|
||||
self.df["time"], self.df[f"rot_current.{c}"], label=f"Rot Current {c}"
|
||||
)
|
||||
|
||||
self.ax.plot(
|
||||
self.df["time"], self.df[f"angvel_target.{c}"], label=f"AngVel Target {c}"
|
||||
)
|
||||
self.ax.plot(
|
||||
self.df["time"], self.df[f"mot_{coord}"].clip(-1, 1), label=f"mot_{coord}"
|
||||
self.df["time"], self.df[f"angvel_current.{c}"], label=f"AngVel Current {c}"
|
||||
)
|
||||
|
||||
self.ax.plot(
|
||||
self.df["time"],
|
||||
self.df[f"mot_target.{c}"],
|
||||
label=f"Mot Target {c}",
|
||||
linestyle="--",
|
||||
)
|
||||
self.ax.plot(
|
||||
self.df["time"],
|
||||
self.df[f"mot_current.{c}"],
|
||||
label=f"Mot Current {c}",
|
||||
alpha=0.7,
|
||||
)
|
||||
# self.ax.plot(
|
||||
# self.df["time"],
|
||||
# self.df[f"dmot_{coord}"].clip(-1, 1),
|
||||
# label=f"desired mot_{coord}",
|
||||
# linestyle="--",
|
||||
# )
|
||||
|
||||
self.ax.set_xlabel("Time (s)")
|
||||
self.ax.set_ylabel("Value")
|
||||
self.ax.set_title(f"{self.current_file.name} — {coord.upper()} Axis")
|
||||
self.ax.legend(loc="upper right")
|
||||
self.ax.grid(True)
|
||||
self.ax.set_title(f"{self.current_file.name} — {c.upper()} Axis")
|
||||
self.ax.legend(loc="upper right", fontsize="small")
|
||||
self.ax.grid(True, alpha=0.3)
|
||||
|
||||
ymin, ymax = self.ax.get_ylim()
|
||||
self.ax.set_ylim(min(ymin, -1.1), max(ymax, 1.1))
|
||||
self.canvas.draw()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,16 +0,0 @@
|
|||
|
||||
layers = [
|
||||
{ type = "Rate", max_rate = 3.14, kp = [
|
||||
0.01,
|
||||
0.1,
|
||||
0.01,
|
||||
], ki = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], kd = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], frequency = 600.0 },
|
||||
]
|
||||
|
|
@ -1,9 +1,8 @@
|
|||
|
||||
layers = [
|
||||
{ type = "Rate", max_rate = 3.14, kp = [
|
||||
0.02,
|
||||
0.2,
|
||||
0.02,
|
||||
stack = { max_rate = 3.14, rotation_pid = { kp = [
|
||||
0.5,
|
||||
0.5,
|
||||
0.5,
|
||||
], ki = [
|
||||
0.0,
|
||||
0.0,
|
||||
|
|
@ -12,5 +11,16 @@ layers = [
|
|||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], frequency = 600.0 },
|
||||
]
|
||||
], frequency = 100.0 }, rate_pid = { kp = [
|
||||
0.03,
|
||||
0.3,
|
||||
0.03,
|
||||
], ki = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], kd = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], frequency = 600.0 } }
|
||||
|
|
|
|||
|
|
@ -0,0 +1,26 @@
|
|||
|
||||
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.03,
|
||||
0.3,
|
||||
0.03,
|
||||
], ki = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], kd = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], frequency = 600.0 } }
|
||||
|
|
@ -1,9 +1,8 @@
|
|||
|
||||
layers = [
|
||||
{ type = "Rate", max_rate = 3.14, kp = [
|
||||
0.02,
|
||||
0.2,
|
||||
0.02,
|
||||
stack = { max_rate = 3.14, rotation_pid = { kp = [
|
||||
1.0,
|
||||
1.0,
|
||||
1.0,
|
||||
], ki = [
|
||||
0.0,
|
||||
0.0,
|
||||
|
|
@ -12,5 +11,16 @@ layers = [
|
|||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], frequency = 600.0 },
|
||||
]
|
||||
], frequency = 50.0 }, rate_pid = { kp = [
|
||||
0.03,
|
||||
0.3,
|
||||
0.03,
|
||||
], ki = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], kd = [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
], frequency = 600.0 } }
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
not/jsontest.json
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -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": 0,
|
||||
"pitch": 0
|
||||
},
|
||||
"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,
|
||||
"pitch": 0
|
||||
},
|
||||
"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,
|
||||
"pitch": 0
|
||||
},
|
||||
"mode": "Rotation"
|
||||
},
|
||||
"time": 20
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -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": 0,
|
||||
"yaw": 3.14,
|
||||
"pitch": 0
|
||||
},
|
||||
"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": 0,
|
||||
"yaw": -1.5,
|
||||
"pitch": 0
|
||||
},
|
||||
"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": 0,
|
||||
"yaw": 1.5,
|
||||
"pitch": 0
|
||||
},
|
||||
"mode": "Rotation"
|
||||
},
|
||||
"time": 20
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -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": 0,
|
||||
"yaw": 0,
|
||||
"pitch": 3.14
|
||||
},
|
||||
"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": 0,
|
||||
"yaw": 0,
|
||||
"pitch": -1.5
|
||||
},
|
||||
"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": 0,
|
||||
"yaw": 0,
|
||||
"pitch": 1.5
|
||||
},
|
||||
"mode": "Rotation"
|
||||
},
|
||||
"time": 20
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -1,3 +1,4 @@
|
|||
use nalgebra::{vector, Vector3};
|
||||
use rapier3d::prelude::*;
|
||||
use serde::Deserialize;
|
||||
|
||||
|
|
@ -6,25 +7,31 @@ pub struct PidConfig {
|
|||
pub kp: [f32; 3],
|
||||
pub ki: [f32; 3],
|
||||
pub kd: [f32; 3],
|
||||
pub frequency: f32,
|
||||
}
|
||||
|
||||
impl PidConfig {
|
||||
pub fn p_vec(&self) -> Vector3<f32> {
|
||||
vector![self.kp[0], self.kp[1], self.kp[2]]
|
||||
}
|
||||
pub fn i_vec(&self) -> Vector3<f32> {
|
||||
vector![self.ki[0], self.ki[1], self.ki[2]]
|
||||
}
|
||||
pub fn d_vec(&self) -> Vector3<f32> {
|
||||
vector![self.kd[0], self.kd[1], self.kd[2]]
|
||||
}
|
||||
}
|
||||
|
||||
/// Now each layer is explicitly typed
|
||||
#[derive(Debug, Deserialize, Clone)]
|
||||
#[serde(tag = "type")]
|
||||
pub enum LayerConfig {
|
||||
/// Controls angular velocity (Input: joystick/previous layer -> Output: torque)
|
||||
Rate {
|
||||
#[serde(flatten)]
|
||||
pid: PidConfig,
|
||||
max_rate: f32,
|
||||
frequency: f32,
|
||||
},
|
||||
/// Controls orientation (Input: joystick -> Output: desired angular velocity)
|
||||
Angle {
|
||||
#[serde(flatten)]
|
||||
pid: PidConfig,
|
||||
max_angle: f32,
|
||||
frequency: f32,
|
||||
},
|
||||
pub struct ControllerStackConfig {
|
||||
/// PID for the rotation (angle → angular rate) layer
|
||||
pub rotation_pid: PidConfig,
|
||||
|
||||
/// PID for the angular rate (angular rate → torque) layer
|
||||
pub rate_pid: PidConfig,
|
||||
/// Maximum angular rate (rad/s) that joystick input maps to
|
||||
pub max_rate: f32,
|
||||
}
|
||||
|
||||
#[derive(Debug, Deserialize, Clone)]
|
||||
|
|
@ -32,9 +39,8 @@ pub struct SimulationConfig {
|
|||
pub tickrate: f32,
|
||||
pub drone_tick_rate: u64,
|
||||
|
||||
// --- Modular Controller Stack ---
|
||||
// The order of this Vec defines the stack (e.g., [Angle, Rate])
|
||||
pub layers: Vec<LayerConfig>,
|
||||
/// Controller stack
|
||||
pub stack: ControllerStackConfig,
|
||||
|
||||
/// Maps [Roll, Yaw, Pitch] to each of the 4 motors
|
||||
pub motor_map: [[f32; 3]; 4],
|
||||
|
|
@ -47,15 +53,3 @@ pub struct SimulationConfig {
|
|||
pub time_constant: f32,
|
||||
pub mass: f32,
|
||||
}
|
||||
|
||||
impl PidConfig {
|
||||
pub fn p_vec(&self) -> Vector<f32> {
|
||||
vector![self.kp[0], self.kp[1], self.kp[2]]
|
||||
}
|
||||
pub fn i_vec(&self) -> Vector<f32> {
|
||||
vector![self.ki[0], self.ki[1], self.ki[2]]
|
||||
}
|
||||
pub fn d_vec(&self) -> Vector<f32> {
|
||||
vector![self.kd[0], self.kd[1], self.kd[2]]
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@ pub struct Drone {
|
|||
width: f32,
|
||||
height: f32,
|
||||
pub current_throttles: [f32; 4],
|
||||
target_throttles: [f32; 4],
|
||||
last_time: f32,
|
||||
}
|
||||
|
||||
|
|
@ -107,6 +108,7 @@ impl Drone {
|
|||
width,
|
||||
height,
|
||||
current_throttles: [0.0; 4],
|
||||
target_throttles: [0.0; 4],
|
||||
last_time: world.get_time(),
|
||||
};
|
||||
}
|
||||
|
|
@ -127,7 +129,7 @@ impl Drone {
|
|||
}
|
||||
|
||||
fn apply_throttles(&mut self, world: &mut World, dt: f32) {
|
||||
let throttles = self.controller.get_motor_throttles();
|
||||
let throttles = self.target_throttles;
|
||||
|
||||
let drone_rb = world.bodies.get_mut(self.rb_handle).unwrap();
|
||||
|
||||
|
|
@ -186,6 +188,7 @@ impl Drone {
|
|||
self.controller
|
||||
.set_angular_velocity(rb.rotation().inverse().transform_vector(&rb.angvel()));
|
||||
self.controller.set_rotation(*rb.rotation());
|
||||
self.target_throttles = self.controller.get_motor_throttles();
|
||||
}
|
||||
|
||||
pub fn get_angvel(&self, world: &World) -> na::Vector3<f32> {
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@ pub struct RotationInput {
|
|||
pub enum ModeInput {
|
||||
#[default]
|
||||
Acro,
|
||||
Rotation,
|
||||
Navigation,
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,10 +1,10 @@
|
|||
#![allow(dead_code)]
|
||||
|
||||
use nalgebra::{self as na, vector};
|
||||
use nalgebra as na;
|
||||
use std::{any::Any, f32};
|
||||
|
||||
use crate::drone::input::ModeInput;
|
||||
use crate::drone::stacked::modules::ModuleRuntime;
|
||||
use crate::drone::JoystickInput;
|
||||
use crate::drone::{controller::DroneController, input::Input};
|
||||
|
||||
use crate::config::*;
|
||||
|
|
@ -13,7 +13,7 @@ pub mod mixer;
|
|||
pub mod modules;
|
||||
|
||||
use mixer::MotorMixer;
|
||||
use modules::{ControllerModule, PidProcessor};
|
||||
use modules::*;
|
||||
|
||||
pub struct DroneState {
|
||||
pub rotation: na::UnitQuaternion<f32>,
|
||||
|
|
@ -21,9 +21,11 @@ pub struct DroneState {
|
|||
}
|
||||
|
||||
pub struct StackedController {
|
||||
modules: Vec<ModuleRuntime>,
|
||||
config: SimulationConfig,
|
||||
pub rotation_rt: ModuleRuntime<RotationController>,
|
||||
pub rate_rt: ModuleRuntime<AngularRateController>,
|
||||
|
||||
mixer: MotorMixer,
|
||||
config: SimulationConfig,
|
||||
|
||||
// State
|
||||
drone_state: DroneState,
|
||||
|
|
@ -33,54 +35,19 @@ pub struct StackedController {
|
|||
}
|
||||
|
||||
impl StackedController {
|
||||
pub fn set_input(&mut self, inp: Input) {
|
||||
self.input = inp;
|
||||
}
|
||||
pub fn new(config: SimulationConfig) -> Self {
|
||||
let mut modules = Vec::new();
|
||||
|
||||
for layer in &config.layers {
|
||||
let (module, freq) = match layer {
|
||||
LayerConfig::Angle {
|
||||
pid,
|
||||
max_angle,
|
||||
frequency,
|
||||
} => (
|
||||
ControllerModule::Rotation {
|
||||
processor: PidProcessor::new(pid),
|
||||
max_angle: *max_angle,
|
||||
},
|
||||
*frequency,
|
||||
),
|
||||
LayerConfig::Rate {
|
||||
pid,
|
||||
max_rate,
|
||||
frequency,
|
||||
} => (
|
||||
ControllerModule::AngularRate {
|
||||
processor: PidProcessor::new(pid),
|
||||
max_rate: *max_rate,
|
||||
},
|
||||
*frequency,
|
||||
),
|
||||
};
|
||||
|
||||
modules.push(ModuleRuntime {
|
||||
module,
|
||||
target_dt: 1.0 / freq, // Convert Hz to Seconds
|
||||
accumulated_time: 0.0,
|
||||
last_output: na::Vector3::zeros(),
|
||||
});
|
||||
}
|
||||
let rotation_ctrl = RotationController::new(PidProcessor::new(&config.stack.rotation_pid));
|
||||
let rate_ctrl = AngularRateController::new(PidProcessor::new(&config.stack.rate_pid));
|
||||
|
||||
Self {
|
||||
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,
|
||||
max_throttle: 1.0,
|
||||
mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 },
|
||||
},
|
||||
modules, // Now Vec<ModuleRuntime>
|
||||
config,
|
||||
input: Input::default(),
|
||||
drone_state: DroneState {
|
||||
|
|
@ -91,6 +58,10 @@ impl StackedController {
|
|||
current_time: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn set_input(&mut self, inp: Input) {
|
||||
self.input = inp;
|
||||
}
|
||||
}
|
||||
|
||||
impl DroneController for StackedController {
|
||||
|
|
@ -108,37 +79,34 @@ impl DroneController for StackedController {
|
|||
fn get_motor_throttles(&mut self) -> [f32; 4] {
|
||||
let frame_dt = (self.current_time - self.last_time).max(0.0);
|
||||
|
||||
// Initial setpoint comes from the sticks
|
||||
let mut current_setpoint = vector![
|
||||
let angular_rate_setpoint = if self.input.mode != ModeInput::Acro {
|
||||
let rotation_setpoint = if self.input.mode != ModeInput::Rotation {
|
||||
panic!("Not Implemented")
|
||||
} else {
|
||||
// println!("Rotation!");
|
||||
Rotation(na::vector![
|
||||
self.input.rotation.roll,
|
||||
self.input.rotation.yaw,
|
||||
self.input.rotation.pitch,
|
||||
])
|
||||
};
|
||||
self.rotation_rt
|
||||
.update(rotation_setpoint, &self.drone_state, frame_dt)
|
||||
} else {
|
||||
AngularRate(
|
||||
na::vector![
|
||||
self.input.joystick.roll_input,
|
||||
self.input.joystick.yaw_input,
|
||||
self.input.joystick.pitch_input,
|
||||
];
|
||||
] * self.config.stack.max_rate,
|
||||
)
|
||||
};
|
||||
|
||||
for (i, runtime) in self.modules.iter_mut().enumerate() {
|
||||
runtime.accumulated_time += frame_dt;
|
||||
let torque = self
|
||||
.rate_rt
|
||||
.update(angular_rate_setpoint, &self.drone_state, frame_dt);
|
||||
|
||||
if runtime.accumulated_time >= runtime.target_dt {
|
||||
// is_first_layer logic: only true if it's the very first module in the vec
|
||||
let is_first_layer = i == 0;
|
||||
|
||||
runtime.last_output = runtime.module.process(
|
||||
current_setpoint,
|
||||
&self.drone_state,
|
||||
runtime.accumulated_time,
|
||||
is_first_layer,
|
||||
);
|
||||
|
||||
// Subtract to keep timing consistent and account for frame jitter
|
||||
runtime.accumulated_time -= runtime.target_dt;
|
||||
}
|
||||
|
||||
// Pass this module's output (either fresh or cached) to the next module
|
||||
current_setpoint = runtime.last_output;
|
||||
}
|
||||
|
||||
self.mixer
|
||||
.mix(self.input.joystick.throttle_input, current_setpoint)
|
||||
self.mixer.mix(self.input.joystick.throttle_input, torque.0)
|
||||
}
|
||||
|
||||
fn as_any(&self) -> &dyn Any {
|
||||
|
|
|
|||
|
|
@ -2,69 +2,73 @@ use crate::config::PidConfig;
|
|||
use crate::drone::stacked::DroneState;
|
||||
use nalgebra as na;
|
||||
|
||||
pub enum ControllerModule {
|
||||
AngularRate {
|
||||
processor: PidProcessor,
|
||||
max_rate: f32,
|
||||
},
|
||||
Rotation {
|
||||
processor: PidProcessor,
|
||||
max_angle: f32,
|
||||
},
|
||||
pub mod rate;
|
||||
pub mod rot;
|
||||
|
||||
pub use rate::AngularRateController;
|
||||
pub use rot::RotationController;
|
||||
|
||||
pub trait ControllerModule {
|
||||
type Input: Clone + Default;
|
||||
type Output: Clone + Default;
|
||||
|
||||
fn process(&mut self, input: Self::Input, state: &DroneState, dt: f32) -> Self::Output;
|
||||
}
|
||||
|
||||
pub struct ModuleRuntime {
|
||||
pub module: ControllerModule,
|
||||
pub target_dt: f32, // e.g., 0.01 for 100Hz
|
||||
pub struct ModuleRuntime<C>
|
||||
where
|
||||
C: ControllerModule,
|
||||
{
|
||||
pub module: C,
|
||||
pub target_dt: f32,
|
||||
pub accumulated_time: f32,
|
||||
pub last_output: na::Vector3<f32>, // Store the last result to pass down the chain
|
||||
pub last_output: Option<C::Output>,
|
||||
}
|
||||
|
||||
impl ControllerModule {
|
||||
pub fn process(
|
||||
&mut self,
|
||||
setpoint: na::Vector3<f32>,
|
||||
state: &DroneState,
|
||||
dt: f32,
|
||||
is_first_layer: bool,
|
||||
) -> na::Vector3<f32> {
|
||||
match self {
|
||||
ControllerModule::Rotation {
|
||||
processor,
|
||||
max_angle,
|
||||
} => {
|
||||
// Setpoint is -1.0..1.0, scale it to target Radians
|
||||
let target_angles = if is_first_layer {
|
||||
setpoint * *max_angle
|
||||
impl<C> ModuleRuntime<C>
|
||||
where
|
||||
C: ControllerModule,
|
||||
{
|
||||
pub fn update(&mut self, input: C::Input, state: &DroneState, dt: f32) -> C::Output {
|
||||
self.accumulated_time += dt;
|
||||
|
||||
if self.accumulated_time >= self.target_dt {
|
||||
self.accumulated_time = 0.0;
|
||||
|
||||
let output = self.module.process(input, state, self.target_dt);
|
||||
self.last_output = Some(output.clone());
|
||||
output
|
||||
} else {
|
||||
setpoint
|
||||
};
|
||||
|
||||
let (r, p, y) = state.rotation.euler_angles();
|
||||
let current_angles = na::vector![r, y, p];
|
||||
|
||||
// Output of Angle PID = Desired Angular Velocity
|
||||
processor.update(target_angles, current_angles, dt)
|
||||
self.last_output
|
||||
.as_ref()
|
||||
.unwrap_or(&C::Output::default())
|
||||
.clone()
|
||||
}
|
||||
}
|
||||
pub fn new(module: C, frequency: f32) -> Self {
|
||||
Self {
|
||||
module,
|
||||
target_dt: 1.0 / frequency,
|
||||
accumulated_time: 0.0,
|
||||
last_output: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ControllerModule::AngularRate {
|
||||
processor,
|
||||
max_rate,
|
||||
} => {
|
||||
// If Rate is the start of the chain (Acro mode), scale the joystick.
|
||||
// If it's the second layer, the setpoint is already a velocity from the Angle layer.
|
||||
let target_velocity = if is_first_layer {
|
||||
setpoint * *max_rate
|
||||
} else {
|
||||
setpoint
|
||||
};
|
||||
#[derive(Clone, Copy, Debug, Default)]
|
||||
pub struct Position(pub na::Vector3<f32>); // meters
|
||||
|
||||
// Output of Rate PID = Desired Torque/Correction Force
|
||||
processor.update(target_velocity, state.angular_velocity, dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#[derive(Clone, Copy, Debug, Default)]
|
||||
pub struct Velocity(pub na::Vector3<f32>); // m/s
|
||||
|
||||
#[derive(Clone, Copy, Debug, Default)]
|
||||
pub struct Rotation(pub na::Vector3<f32>); // radians
|
||||
|
||||
#[derive(Clone, Copy, Debug, Default)]
|
||||
pub struct AngularRate(pub na::Vector3<f32>); // rad/s
|
||||
|
||||
#[derive(Clone, Copy, Debug, Default)]
|
||||
pub struct Torque(pub na::Vector3<f32>); // control output
|
||||
|
||||
pub struct PidProcessor {
|
||||
kp: na::Vector3<f32>,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,25 @@
|
|||
use crate::drone::stacked::modules::*;
|
||||
|
||||
pub struct AngularRateController {
|
||||
pid: PidProcessor,
|
||||
}
|
||||
|
||||
impl AngularRateController {
|
||||
pub fn new(pid: PidProcessor) -> Self {
|
||||
Self { pid }
|
||||
}
|
||||
}
|
||||
|
||||
impl ControllerModule for AngularRateController {
|
||||
type Input = AngularRate;
|
||||
type Output = Torque;
|
||||
|
||||
fn process(&mut self, input: AngularRate, state: &DroneState, dt: f32) -> Torque {
|
||||
// Scale normalized rate command
|
||||
let target_rate = input.0;
|
||||
|
||||
let output = self.pid.update(target_rate, state.angular_velocity, dt);
|
||||
|
||||
Torque(output)
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
use nalgebra::{Quaternion, UnitQuaternion};
|
||||
|
||||
use crate::drone::stacked::modules::*;
|
||||
|
||||
pub struct RotationController {
|
||||
pid: PidProcessor,
|
||||
}
|
||||
impl RotationController {
|
||||
pub fn new(pid: PidProcessor) -> Self {
|
||||
Self { pid }
|
||||
}
|
||||
}
|
||||
|
||||
impl ControllerModule for RotationController {
|
||||
type Input = Rotation;
|
||||
type Output = AngularRate;
|
||||
|
||||
fn process(&mut self, input: Rotation, state: &DroneState, dt: f32) -> AngularRate {
|
||||
let target_rot = UnitQuaternion::from_scaled_axis(input.0);
|
||||
|
||||
let error_quat = state.rotation.rotation_to(&target_rot);
|
||||
|
||||
let error_vector = error_quat.scaled_axis();
|
||||
|
||||
let output = self.pid.update(error_vector, na::Vector3::zeros(), dt);
|
||||
|
||||
AngularRate(state.rotation.inverse_transform_vector(&output))
|
||||
}
|
||||
}
|
||||
|
|
@ -1,20 +1,22 @@
|
|||
use nalgebra as na;
|
||||
use serde::Serialize;
|
||||
use std::io::Write;
|
||||
|
||||
pub struct CsvLogger {
|
||||
writer: csv::Writer<std::fs::File>,
|
||||
pub struct Logger {
|
||||
writer: std::io::BufWriter<std::fs::File>,
|
||||
}
|
||||
|
||||
impl CsvLogger {
|
||||
impl Logger {
|
||||
pub fn new(path: &str) -> Result<Self, Box<dyn std::error::Error>> {
|
||||
let file = std::fs::File::create(path)?;
|
||||
Ok(Self {
|
||||
writer: csv::Writer::from_writer(file),
|
||||
writer: std::io::BufWriter::new(file),
|
||||
})
|
||||
}
|
||||
|
||||
pub fn log(&mut self, row: &SimLogRow) -> Result<(), Box<dyn std::error::Error>> {
|
||||
self.writer.serialize(row)?;
|
||||
serde_json::to_writer(&mut self.writer, row)?;
|
||||
self.writer.write_all(b"\n")?; // newline-delimited JSON
|
||||
Ok(())
|
||||
}
|
||||
|
||||
|
|
@ -24,51 +26,35 @@ impl CsvLogger {
|
|||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Serialize)]
|
||||
pub struct Vec3Serialize {
|
||||
pub x: f32,
|
||||
pub y: f32,
|
||||
pub z: f32,
|
||||
}
|
||||
|
||||
impl From<na::Vector3<f32>> for Vec3Serialize {
|
||||
fn from(value: na::Vector3<f32>) -> Self {
|
||||
Self {
|
||||
x: value.x,
|
||||
y: value.y,
|
||||
z: value.z,
|
||||
}
|
||||
}
|
||||
}
|
||||
impl From<Vec3Serialize> for na::Vector3<f32> {
|
||||
fn from(value: Vec3Serialize) -> Self {
|
||||
na::vector![value.x, value.y, value.z]
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Serialize)]
|
||||
pub struct SimLogRow {
|
||||
pub time: f32,
|
||||
pub target_x: f32,
|
||||
pub target_y: f32,
|
||||
pub target_z: f32,
|
||||
|
||||
pub current_x: f32,
|
||||
pub current_y: f32,
|
||||
pub current_z: f32,
|
||||
|
||||
pub mot_x: f32,
|
||||
pub mot_y: f32,
|
||||
pub mot_z: f32,
|
||||
|
||||
pub dmot_x: f32,
|
||||
pub dmot_y: f32,
|
||||
pub dmot_z: f32,
|
||||
}
|
||||
|
||||
impl SimLogRow {
|
||||
pub fn from_data(
|
||||
time: f32,
|
||||
target: na::Vector3<f32>,
|
||||
current: na::Vector3<f32>,
|
||||
mot: na::Vector3<f32>,
|
||||
dmot: na::Vector3<f32>,
|
||||
) -> Self {
|
||||
Self {
|
||||
time,
|
||||
target_x: target.x,
|
||||
target_y: target.y,
|
||||
target_z: target.z,
|
||||
|
||||
current_x: current.x,
|
||||
current_y: current.y,
|
||||
current_z: current.z,
|
||||
|
||||
mot_x: mot.x,
|
||||
mot_y: mot.y,
|
||||
mot_z: mot.z,
|
||||
|
||||
dmot_x: dmot.x,
|
||||
dmot_y: dmot.y,
|
||||
dmot_z: dmot.z,
|
||||
}
|
||||
}
|
||||
pub angvel_target: Vec3Serialize,
|
||||
pub angvel_current: Vec3Serialize,
|
||||
pub mot_current: Vec3Serialize,
|
||||
pub mot_target: Vec3Serialize,
|
||||
pub rot_target: Vec3Serialize,
|
||||
pub rot_current: Vec3Serialize,
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,6 +26,10 @@ use std::thread::JoinHandle;
|
|||
|
||||
fn main() {
|
||||
run_batch();
|
||||
// run(
|
||||
// &"./inputs/rot.json".into(),
|
||||
// &"./configurations/final/sim_std_mot_std_rate_03.toml".into(),
|
||||
// );
|
||||
}
|
||||
|
||||
fn run_batch() {
|
||||
|
|
@ -82,7 +86,7 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
|
|||
config.mass,
|
||||
);
|
||||
|
||||
let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name);
|
||||
let result_file = format!("{}/{}_{}.json", RESULTS_DIR, input_name, config_name);
|
||||
|
||||
let mut sim = Simulation::new(
|
||||
drone,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,16 @@
|
|||
use nalgebra as na;
|
||||
|
||||
fn main() {
|
||||
let rot = na::UnitQuaternion::from_scaled_axis(na::vector![3.14, 1.5, 0.0]);
|
||||
|
||||
let target = na::UnitQuaternion::from_scaled_axis(na::vector![-1.0, 0.0, 0.0]);
|
||||
|
||||
let error_quat = rot.rotation_to(&target);
|
||||
|
||||
let error_vector = error_quat.scaled_axis();
|
||||
|
||||
println!("start: {:}", rot.scaled_axis());
|
||||
println!("target: {:}", target.scaled_axis());
|
||||
println!("error: {:}", error_vector);
|
||||
println!("final: {:}", (error_quat * rot).scaled_axis());
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
use macroquad::prelude as mq;
|
||||
use nalgebra as na;
|
||||
use nalgebra::{self as na, vector};
|
||||
use rapier3d::prelude as rp;
|
||||
use std::error::Error;
|
||||
|
||||
|
|
@ -10,7 +10,7 @@ use crate::{
|
|||
Drone,
|
||||
},
|
||||
engine::World,
|
||||
logger::CsvLogger,
|
||||
logger::{Logger, SimLogRow},
|
||||
rendering::Renderer,
|
||||
};
|
||||
|
||||
|
|
@ -37,7 +37,7 @@ pub struct Simulation {
|
|||
pub world: World,
|
||||
pub mode: SimMode,
|
||||
|
||||
logger: Option<CsvLogger>,
|
||||
logger: Option<Logger>,
|
||||
drone_tick_rate: u64,
|
||||
}
|
||||
|
||||
|
|
@ -50,7 +50,7 @@ impl Simulation {
|
|||
drone_tick_rate: u64,
|
||||
) -> Self {
|
||||
let logger = match &results_file {
|
||||
Some(path) => Some(CsvLogger::new(path).unwrap()),
|
||||
Some(path) => Some(Logger::new(path).unwrap()),
|
||||
None => None,
|
||||
};
|
||||
|
||||
|
|
@ -159,12 +159,22 @@ impl Simulation {
|
|||
}
|
||||
self.drone.process_tick(&mut self.world);
|
||||
|
||||
let target_angular_vel: na::Vector3<f32> = na::vector![
|
||||
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,
|
||||
] * 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>()
|
||||
{
|
||||
target_angular_vel = cont.rotation_rt.last_output.unwrap_or_default().0;
|
||||
target_mot = cont.rate_rt.last_output.unwrap_or_default().0;
|
||||
}
|
||||
let applied_motor_diff: na::Vector3<f32> = na::vector![
|
||||
(self.drone.current_throttles[1] + self.drone.current_throttles[2]
|
||||
- self.drone.current_throttles[0]
|
||||
|
|
@ -179,17 +189,26 @@ impl Simulation {
|
|||
|
||||
if let Some(logger) = &mut self.logger {
|
||||
logger
|
||||
.log(&crate::logger::SimLogRow::from_data(
|
||||
self.world.get_time(),
|
||||
target_angular_vel,
|
||||
self.drone
|
||||
.log(&SimLogRow {
|
||||
time: self.world.get_time(),
|
||||
angvel_target: target_angular_vel.into(),
|
||||
angvel_current: self
|
||||
.drone
|
||||
.get_rot(&self.world)
|
||||
.inverse()
|
||||
.transform_vector(&self.drone.get_angvel(&self.world))
|
||||
.into(),
|
||||
applied_motor_diff,
|
||||
na::vector![0.0, 0.0, 0.0],
|
||||
))
|
||||
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
|
||||
])
|
||||
.scaled_axis()
|
||||
.into(),
|
||||
mot_current: applied_motor_diff.into(),
|
||||
mot_target: target_mot.into(),
|
||||
})
|
||||
.unwrap();
|
||||
}
|
||||
Ok(StepOutcome::Continue)
|
||||
|
|
|
|||
Loading…
Reference in New Issue