43 lines
1.1 KiB
Rust
43 lines
1.1 KiB
Rust
|
use nalgebra as na;
|
||
|
|
|
||
|
|
pub struct MotorMixer {
|
||
|
|
pub motor_map: [[f32; 3]; 4], // roll, yaw, pitch
|
||
|
|
pub min_throttle: f32,
|
||
|
|
pub max_throttle: f32,
|
||
|
|
}
|
||
|
|
|
||
|
|
impl MotorMixer {
|
||
|
|
pub fn mix(&self, throttle: f32, desired: na::Vector3<f32>) -> ([f32; 4], bool) {
|
||
|
|
let mut delta = [0.0f32; 4];
|
||
|
|
|
||
|
|
// 1. Torque-only contribution
|
||
|
|
for i in 0..4 {
|
||
|
|
delta[i] = self.motor_map[i][0] * desired.x
|
||
|
|
+ self.motor_map[i][1] * desired.y
|
||
|
|
+ self.motor_map[i][2] * desired.z;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 2. Compute allowable scaling
|
||
|
|
let mut scale = 1.0f32;
|
||
|
|
|
||
|
|
for i in 0..4 {
|
||
|
|
if delta[i] > 0.0 {
|
||
|
|
scale = scale.min((self.max_throttle - throttle) / delta[i]);
|
||
|
|
} else if delta[i] < 0.0 {
|
||
|
|
scale = scale.min((self.min_throttle - throttle) / delta[i]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
scale = scale.clamp(0.0, 1.0);
|
||
|
|
|
||
|
|
// 3. Apply
|
||
|
|
let mut motors = [0.0f32; 4];
|
||
|
|
for i in 0..4 {
|
||
|
|
motors[i] = throttle + scale * delta[i];
|
||
|
|
}
|
||
|
|
|
||
|
|
let saturated = scale < 1.0;
|
||
|
|
(motors, saturated)
|
||
|
|
}
|
||
|
|
}
|