58 lines
1.4 KiB
Rust
58 lines
1.4 KiB
Rust
use nalgebra::{vector, Vector3};
|
|
use rapier3d::prelude::*;
|
|
use serde::Deserialize;
|
|
|
|
#[derive(Debug, Deserialize, Clone)]
|
|
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)]
|
|
pub struct ControllerStackConfig {
|
|
/// PID for the rotation (angle → angular rate) layer
|
|
pub rotation_pid: PidConfig,
|
|
|
|
pub acceleration_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)]
|
|
pub struct SimulationConfig {
|
|
pub tickrate: f32,
|
|
pub drone_tick_rate: u64,
|
|
|
|
/// Controller stack
|
|
pub stack: ControllerStackConfig,
|
|
|
|
/// Maps [Roll, Yaw, Pitch] to each of the 4 motors
|
|
pub motor_map: [[f32; 3]; 4],
|
|
|
|
// Motors & Physics
|
|
pub max_thrust: f32,
|
|
pub max_torque: f32,
|
|
|
|
#[serde(default)]
|
|
pub time_constant: f32,
|
|
pub mass: f32,
|
|
}
|