26 lines
569 B
Rust
26 lines
569 B
Rust
use crate::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.angvel, dt);
|
|
|
|
Torque(output)
|
|
}
|
|
}
|