Cansat
/
RustPhysicsMQ
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

before split into lib

This commit is contained in:
franchioping 2026-03-11 11:39:58 +00:00
parent 9b0dc51d4c
commit 8f8b129a6d
10 changed files with 147 additions and 1200124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
analyze.py
View File

@ -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
# The keys follow the pattern: category.axis
self.ax.plot(
self.df["time"], self.df[f"rot_target.{c}"], label=f"Rot Target {c}"
)
self.ax.plot(
self.df["time"], self.df[f"rot_current.{c}"], label=f"Rot Current {c}"
)
to_plot = [
("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"),
]
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"angvel_current.{c}"], label=f"AngVel Current {c}"
)
for name, label in to_plot:
self.ax.plot(self.df["time"], self.df[f"{name}.{c}"], label=f"{label} {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"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}",
# )
self.ax.set_xlabel("Time (s)")
self.ax.set_ylabel("Value")

26
configurations/pid_cont/rate_025.toml
View File

@ -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 } }

18
configurations/pid_cont/rate_030.toml
View File

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

2
configurations/sim/sim_std.toml
View File

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

19
src/drone.rs
View File

@ -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();
}
}

31
src/drone/stacked.rs
View File

@ -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 {

6
src/drone/stacked/mixer.rs
View File

@ -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;
max_delta *= throttle_range_len / delta_dif * 0.9;
min_delta *= throttle_range_len / delta_dif * 0.9;
scale *= throttle_range_len / delta_dif * 0.99;
max_delta *= throttle_range_len / delta_dif * 0.99;
min_delta *= throttle_range_len / delta_dif * 0.99;
}
let lim_throttle = throttle.clamp(

3
src/logger.rs
View File

@ -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,

74
src/simulation.rs
View File

@ -159,33 +159,8 @@ impl Simulation {
}
self.drone.process_tick(&mut self.world);
let mut target_angular_vel: na::Vector3<f32> = na::vector![
current_input.joystick.roll_input,
current_input.joystick.pitch_input,
current_input.joystick.yaw_input,
] * 3.14;
/* Logging */
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![
current_input.rotation.roll,
current_input.rotation.pitch,
current_input.rotation.yaw
])
rot_target: na::UnitQuaternion::from_scaled_axis(target_rot)
.scaled_axis()
.into(),
mot_current: applied_motor_diff.into(),
@ -222,6 +232,8 @@ impl Simulation {
})
.unwrap();
}
}
Ok(StepOutcome::Continue)
}

1200003
test.txt
View File

File diff suppressed because it is too large Load Diff