1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127

use super::MotorControl;
use crate::ESC;
use nalgebra::Vector3;
use std::f32::consts::SQRT_2;

pub struct QuadMotorControl<E> {
    pub m: f32,
    pub k_m: f32,
    pub k_f: f32,
    /// Perpendicular distance to axes (in meters)
    pub l: f32,
    pub min_thrust: f32,
    pub max_thrust: f32,
    pub motors: [E; 4],
}

impl<E: ESC<f32>> QuadMotorControl<E> {
    // Create a new `QuadMotor` from the quad's mass (in grams) and rotor to rotor length (in meters).
    pub fn new(m: f32, length: f32, max_thrust: f32, min_thrust: f32, motors: [E; 4]) -> Self {
        Self {
            k_f: 1.,
            k_m: 1.,
            m,
            l: length / (2. * SQRT_2),
            motors,
            min_thrust,
            max_thrust,
        }
    }

    /// Calculate the thrust on each propeller (in N)
    /// needed to command an angular acceleration (in m/s^2)
    /// and thrust acceleration (in m/s^2)
    /// with the moment of inertia (kgm^2).
    pub fn thrust_from_acceleration(
        &self,
        acceleration: Vector3<f32>,
        thrust_acceleration: f32,
        moment_of_inertia: &Vector3<f32>,
    ) -> [f32; 4] {
        let angular_velocity = self.angular_velocity_from_acceleration(
            acceleration,
            thrust_acceleration,
            moment_of_inertia,
        );
        self.thrust_from_angular_velocity(angular_velocity)
    }

    /// Calculate the angular velocity on each propeller (in m/s)
    /// needed to command a torque (in Nm) and thrust acceleration (in m/s^2)
    /// with the moment of inertia (kgm^2).
    pub fn angular_velocity(
        &self,
        torque: Vector3<f32>,
        thrust_acceleration: f32,
        moment_of_inertia: &Vector3<f32>,
    ) -> [f32; 4] {
        let acceleration = torque.zip_map(moment_of_inertia, |t, moi| acceleration(t, moi));
        self.angular_velocity_from_acceleration(
            acceleration,
            thrust_acceleration,
            moment_of_inertia,
        )
    }

    /// Calculate the angular velocity on each propeller (in m/s) needed to command an angular acceleration (in m/s^2) and thrust acceleration (in m/s^2).
    pub fn angular_velocity_from_acceleration(
        &self,
        acceleration: Vector3<f32>,
        thrust_acceleration: f32,
        moment_of_inertia: &Vector3<f32>,
    ) -> [f32; 4] {
        // Torque (Nm)
        let c_bar = -thrust_acceleration * self.m / self.k_f;
        let p_bar = acceleration.x * moment_of_inertia.x / (self.k_f * self.l);
        let q_bar = acceleration.y * moment_of_inertia.y / (self.k_f * self.l);
        let r_bar = acceleration.z * moment_of_inertia.z / self.k_m;

        let omega_4 = (c_bar + p_bar - r_bar - q_bar) / 4.;
        let omega_3 = (r_bar - p_bar) / 2. + omega_4;
        let omega_2 = (c_bar - p_bar) / 2. - omega_3;
        let omega_1 = c_bar - omega_2 - omega_3 - omega_4;

        [
            -(omega_1.sqrt()),
            omega_2.sqrt(),
            -(omega_3.sqrt()),
            omega_4.sqrt(),
        ]
    }

    pub fn thrust_from_angular_velocity(&self, angular_velocity: [f32; 4]) -> [f32; 4] {
        angular_velocity.map(|v| self.k_f * v.powi(2))
    }

    pub fn output_thrust(&mut self, thrust: [f32; 4]) {
        for (esc, thrust) in self.motors.iter_mut().zip(thrust) {
            let actuation =
                (thrust - self.min_thrust) * 2. / (self.max_thrust - self.min_thrust) + 1.;
            esc.output(thrust);
        }
    }
}

fn acceleration(net_torque: f32, moment_of_inertia: f32) -> f32 {
    net_torque / moment_of_inertia
}

impl<E: ESC<f32>> MotorControl for QuadMotorControl<E> {
    fn arm(&mut self) {
        for motor in &mut self.motors {
            motor.arm();
        }
    }

    fn motor_control(
        &mut self,
        torque: Vector3<f32>,
        acceleration: f32,
        moment_of_inertia: &Vector3<f32>,
    ) {
        let angular_velocity = self.angular_velocity(torque, acceleration, moment_of_inertia);
        let thrust = self.thrust_from_angular_velocity(angular_velocity);

        self.output_thrust(thrust);
    }
}