embedded_flight_control/
body_rate.rs

1use nalgebra::Vector3;
2
3pub struct BodyRateController {
4    body_rate_k_p: Vector3<f32>,
5    moi: Vector3<f32>,
6    max_torque: f32,
7}
8
9impl Default for BodyRateController {
10    fn default() -> Self {
11        Self {
12            body_rate_k_p: Vector3::new(20., 20., 5.),
13            moi: Vector3::new(0.005, 0.005, 0.01),
14            max_torque: 1.,
15        }
16    }
17}
18
19impl BodyRateController {
20    /// Generate the roll, pitch, yaw moment commands in the body frame in Newtons*meters
21    pub fn body_rate_control(
22        &self,
23        body_rate_cmd: Vector3<f32>,
24        body_rate: Vector3<f32>,
25    ) -> Vector3<f32> {
26        let taus = mul_array(
27            self.moi,
28            mul_array(self.body_rate_k_p, body_rate_cmd - body_rate),
29        );
30        let taus_mod = taus.norm();
31
32        if taus_mod > self.max_torque {
33            taus * self.max_torque / taus_mod
34        } else {
35            taus
36        }
37    }
38}
39
40fn mul_array(lhs: Vector3<f32>, rhs: Vector3<f32>) -> Vector3<f32> {
41    lhs.zip_map(&rhs, |l, r| l * r)
42}
embedded_flight_control/body_rate.rs

embedded_flight_control/
body_rate.rs
