embedded_flight_motors/matrix/
mod.rs

1use crate::ESC;
2use embedded_flight_core::{filter::LowPassFilter, MotorOutput};
3use num_traits::{Float, FloatConst, FromPrimitive, NumCast};
4
5mod motor;
6pub use motor::Motor;
7
8pub struct Limit {
9    pub roll: bool,
10    pub pitch: bool,
11    pub yaw: bool,
12    pub throttle_lower: bool,
13    pub throttle_upper: bool,
14}
15
16pub struct MotorMatrix<E, T, const N: usize> {
17    pub motors: [Motor<E, T>; N],
18    // In -1..1
19    throttle_avg_max: T,
20    throttle_filter: LowPassFilter<T>,
21    rpy_scale: T,
22    throttle_thrust_max: T,
23    loop_rate: T,
24    // choice between highest and second highest motor output for output mixing (0 ~ 1). Zero is normal operation
25    pub thrust_boost_ratio: T,
26    pub compensation_gain: T,
27}
28
29impl<E, T> MotorMatrix<E, T, 4>
30where
31E: ESC,
32    E::Output: NumCast,
33    T: Float + FloatConst + FromPrimitive,
34{
35    /// Create a new `MotorMatrix` for a quad-copter with 4 ESC motors.
36    pub fn quad(a: E, b: E, c: E, d: E) -> Self {
37        Self::from_motors([
38            Motor::from_angle(a, T::from_i8(90).unwrap(), T::from_i8(1).unwrap()),
39            Motor::from_angle(b, T::from_i8(-90).unwrap(), T::from_i8(1).unwrap()),
40            Motor::from_angle(c, T::from_i8(0).unwrap(), T::from_i8(-1).unwrap()),
41            Motor::from_angle(d, T::from_i16(180).unwrap(), T::from_i8(-1).unwrap()),
42        ])
43    }
44}
45
46impl<E, T, const N: usize> MotorMatrix<E, T, N>
47where
48    E: ESC,
49    E::Output: NumCast,
50    T: Float + FloatConst + FromPrimitive,
51{
52    pub fn from_motors(motors: [Motor<E, T>; N]) -> Self {
53        Self {
54            motors,
55            throttle_avg_max: T::zero(),
56            throttle_filter: LowPassFilter::default(),
57            rpy_scale: T::one(),
58            throttle_thrust_max: T::one(),
59            loop_rate: T::zero(),
60            thrust_boost_ratio: T::zero(),
61            compensation_gain: T::one(),
62        }
63    }
64
65    pub fn arm(&mut self) {
66        for motor in &mut self.motors {
67            motor.esc.arm();
68        }
69    }
70
71    pub fn output(&mut self, output: MotorOutput<T>) -> Limit {
72        self.output_with_throttle(output, T::zero())
73    }
74
75    pub fn output_with_throttle(&mut self, output: MotorOutput<T>, throttle: T) -> Limit {
76        let roll_thrust = (output.control[0] + output.feed_forward[0]) * self.compensation_gain;
77        let pitch_thrust = (output.control[1] + output.feed_forward[1]) * self.compensation_gain;
78        let yaw_thrust = (output.control[2] + output.feed_forward[2]) * self.compensation_gain;
79        let mut throttle_thrust =
80            self.throttle_filter
81                .filter(throttle, T::zero(), T::one() / self.loop_rate);
82
83        let throttle_thrust_max = self.thrust_boost_ratio
84            + (T::one() - self.thrust_boost_ratio)
85                * self.throttle_thrust_max
86                * self.compensation_gain;
87
88        // Throttle providing maximum roll, pitch and yaw range without climbing
89        let mut throttle_thrust_best_rpy = T::from_f32(0.5).unwrap().min(self.throttle_avg_max);
90
91        let mut limit = Limit {
92            roll: false,
93            pitch: false,
94            yaw: false,
95            throttle_lower: false,
96            throttle_upper: false,
97        };
98
99        // Sanity check throttle is above zero and below current limited throttle
100        if throttle_thrust <= T::zero() {
101            throttle_thrust = T::zero();
102            limit.throttle_lower = true;
103        }
104        if throttle_thrust >= throttle_thrust_max {
105            throttle_thrust = throttle_thrust_max;
106            limit.throttle_upper = true;
107        }
108
109        let mut yaw_allowed = T::one();
110
111        let mut rp_low = T::one(); // lowest thrust value
112        let mut rp_high = -T::one(); // highest thrust value
113        for motor in &mut self.motors {
114            // calculate the thrust outputs for roll and pitch
115            motor.thrust_rpyt_out = roll_thrust * motor.factor[0] + pitch_thrust * motor.factor[1];
116            // record lowest roll + pitch command
117            if motor.thrust_rpyt_out < rp_low {
118                rp_low = motor.thrust_rpyt_out;
119            }
120            // record highest roll + pitch command
121            if motor.thrust_rpyt_out > rp_high {
122                rp_high = motor.thrust_rpyt_out;
123            }
124
125            // Check the maximum yaw control that can be used on this channel
126            // Exclude any lost motors if thrust boost is enabled
127            if motor.factor[2] == T::zero() {
128                if (yaw_thrust * motor.factor[2]) >= T::zero() {
129                    yaw_allowed = yaw_allowed.min(
130                        ((T::one()
131                            - (throttle_thrust_best_rpy + motor.thrust_rpyt_out).max(T::zero()))
132                            / motor.factor[2])
133                            .abs(),
134                    );
135                } else {
136                    yaw_allowed = yaw_allowed.min(
137                        ((throttle_thrust_best_rpy + motor.thrust_rpyt_out.min(T::zero()))
138                            / motor.factor[2])
139                            .abs(),
140                    );
141                }
142            }
143        }
144
145        let mut rpy_low = T::one(); // lowest thrust value
146        let mut rpy_high = -T::one(); // highest thrust value
147        for motor in &mut self.motors {
148            motor.thrust_rpyt_out = motor.thrust_rpyt_out + yaw_thrust * motor.factor[2];
149
150            // record lowest roll + pitch + yaw command
151            if motor.thrust_rpyt_out < rpy_low {
152                rpy_low = motor.thrust_rpyt_out;
153            }
154
155            // record highest roll + pitch + yaw command
156            // Exclude any lost motors if thrust boost is enabled
157            if motor.thrust_rpyt_out > rpy_high {
158                rpy_high = motor.thrust_rpyt_out;
159            }
160        }
161
162        self.rpy_scale = if rpy_high - rpy_low > T::one() {
163            T::one() / (rpy_high - rpy_low)
164        } else {
165            self.rpy_scale.min(-self.throttle_avg_max / rpy_low)
166        };
167        self.throttle_avg_max = self
168            .throttle_avg_max
169            .max(throttle_thrust)
170            .min(self.throttle_thrust_max);
171
172        // calculate how close the motors can come to the desired throttle
173        rpy_high = rpy_high * self.rpy_scale;
174        rpy_low = rp_low * self.rpy_scale;
175        throttle_thrust_best_rpy = -rpy_low;
176
177        // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
178        let mut thr_adj = throttle_thrust - throttle_thrust_best_rpy;
179
180        if self.rpy_scale < T::one() {
181            // Full range is being used by roll, pitch, and yaw.
182            limit.roll = true;
183            limit.pitch = true;
184            limit.yaw = true;
185            if thr_adj > T::zero() {
186                limit.throttle_upper = true;
187            }
188            thr_adj = T::zero();
189        } else {
190            if thr_adj < T::zero() {
191                // Throttle can't be reduced to desired value
192                // todo: add lower limit flag and ensure it is handled correctly in altitude controller
193                thr_adj = T::zero();
194            } else if thr_adj > T::one() - (throttle_thrust_best_rpy + rpy_high) {
195                // Throttle can't be increased to desired value
196                thr_adj = T::one() - (throttle_thrust_best_rpy + rpy_high);
197                limit.throttle_upper = true;
198            }
199        }
200
201        // add scaled roll, pitch, constrained yaw and throttle for each motor
202        let throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
203        for motor in &mut self.motors {
204            motor.thrust_rpyt_out = (throttle_thrust_best_plus_adj * motor.throttle_factor)
205                + (self.rpy_scale * motor.thrust_rpyt_out);
206
207            motor.esc.output(<<E as ESC>::Output as NumCast>::from(motor.thrust_rpyt_out).unwrap())
208        }
209
210        limit
211    }
212}
embedded_flight_motors/matrix/mod.rs

embedded_flight_motors/matrix/
mod.rs
