120 lines
3.0 KiB
Rust
120 lines
3.0 KiB
Rust
use crate::DroneState;
|
|
use crate::PidConfig;
|
|
use nalgebra as na;
|
|
|
|
pub mod acceleration;
|
|
pub mod angular_rate;
|
|
pub mod position;
|
|
pub mod rotation;
|
|
pub mod velocity;
|
|
|
|
pub use acceleration::AccelerationController;
|
|
pub use angular_rate::AngularRateController;
|
|
pub use position::PositionController;
|
|
pub use rotation::RotationController;
|
|
pub use velocity::VelocityController;
|
|
|
|
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<C>
|
|
where
|
|
C: ControllerModule,
|
|
{
|
|
pub module: C,
|
|
pub target_dt: f32,
|
|
pub accumulated_time: f32,
|
|
pub last_output: Option<C::Output>,
|
|
}
|
|
|
|
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 {
|
|
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,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
pub struct Position(pub na::Vector3<f32>); // meters
|
|
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
pub struct Velocity(pub na::Vector3<f32>); // m/s
|
|
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
pub struct Acceleration(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
|
|
|
|
#[derive(Clone, Copy, Debug, Default)]
|
|
pub struct Throttle(pub f32);
|
|
|
|
pub struct PidProcessor {
|
|
kp: na::Vector3<f32>,
|
|
ki: na::Vector3<f32>,
|
|
kd: na::Vector3<f32>,
|
|
integral: na::Vector3<f32>,
|
|
last_error: na::Vector3<f32>,
|
|
}
|
|
|
|
impl PidProcessor {
|
|
pub fn new(c: &PidConfig) -> Self {
|
|
Self {
|
|
kp: c.p_vec(),
|
|
ki: c.i_vec(),
|
|
kd: c.d_vec(),
|
|
integral: na::Vector3::zeros(),
|
|
last_error: na::Vector3::zeros(),
|
|
}
|
|
}
|
|
|
|
pub fn update(
|
|
&mut self,
|
|
target: na::Vector3<f32>,
|
|
current: na::Vector3<f32>,
|
|
dt: f32,
|
|
) -> na::Vector3<f32> {
|
|
let error = target - current;
|
|
self.integral += error * dt;
|
|
let derivative = (error - self.last_error) / dt;
|
|
self.last_error = error;
|
|
|
|
error.component_mul(&self.kp)
|
|
+ self.integral.component_mul(&self.ki)
|
|
+ derivative.component_mul(&self.kd)
|
|
}
|
|
}
|