veloxity_core/mixer/
matrix.rs

1use crate::command::{
2    OVERRIDE_ATT_SWITCH, OVERRIDE_OFFBOARD_T_INACTIVE, OVERRIDE_OFFBOARD_X_INACTIVE,
3    OVERRIDE_OFFBOARD_Y_INACTIVE, OVERRIDE_OFFBOARD_Z_INACTIVE, OVERRIDE_T, OVERRIDE_THR_SWITCH,
4    OVERRIDE_X, OVERRIDE_Y, OVERRIDE_Z,
5};
6use crate::controller::quad::ControllerOutput;
7use crate::math::prelude::*;
8use crate::mixer::{Mixer, MixerCtx, MixerOutputType, MixerRun, MixerStatus};
9use crate::params::{ParamId, ParamValue, Params};
10use nalgebra::SMatrix;
11
12const NUM_MIXER_OUTPUTS: usize = 10;
13const NUM_MIXERS: i32 = 12;
14const ESC_CALIBRATION_MIXER: i32 = 0;
15const QUAD_PLUS_MIXER: i32 = 1;
16const QUAD_X_MIXER_CHOICE: i32 = 2;
17const HEX_PLUS_MIXER: i32 = 3;
18const HEX_X_MIXER: i32 = 4;
19const OCTO_PLUS_MIXER: i32 = 5;
20const OCTO_X_MIXER: i32 = 6;
21const Y6_MIXER: i32 = 7;
22const X8_MIXER: i32 = 8;
23const FIXEDWING_MIXER: i32 = 9;
24const INVERTED_VTAIL_MIXER: i32 = 10;
25const CUSTOM_MIXER: i32 = 11;
26const X_OVERRIDDEN: u16 = OVERRIDE_ATT_SWITCH | OVERRIDE_X | OVERRIDE_OFFBOARD_X_INACTIVE;
27const Y_OVERRIDDEN: u16 = OVERRIDE_ATT_SWITCH | OVERRIDE_Y | OVERRIDE_OFFBOARD_Y_INACTIVE;
28const Z_OVERRIDDEN: u16 = OVERRIDE_ATT_SWITCH | OVERRIDE_Z | OVERRIDE_OFFBOARD_Z_INACTIVE;
29const T_OVERRIDDEN: u16 = OVERRIDE_THR_SWITCH | OVERRIDE_T | OVERRIDE_OFFBOARD_T_INACTIVE;
30const ATTITUDE_OVERRIDDEN: u16 = X_OVERRIDDEN | Y_OVERRIDDEN | Z_OVERRIDDEN;
31const QUAD_X_OUTPUT_TYPES: [MixerOutputType; NUM_MIXER_OUTPUTS] = [
32    MixerOutputType::Motor,
33    MixerOutputType::Motor,
34    MixerOutputType::Motor,
35    MixerOutputType::Motor,
36    MixerOutputType::Aux,
37    MixerOutputType::Aux,
38    MixerOutputType::Aux,
39    MixerOutputType::Aux,
40    MixerOutputType::Aux,
41    MixerOutputType::Aux,
42];
43const ALL_MOTOR_OUTPUT_TYPES: [MixerOutputType; NUM_MIXER_OUTPUTS] =
44    [MixerOutputType::Motor; NUM_MIXER_OUTPUTS];
45const FIXEDWING_OUTPUT_TYPES: [MixerOutputType; NUM_MIXER_OUTPUTS] = [
46    MixerOutputType::Servo,
47    MixerOutputType::Servo,
48    MixerOutputType::Servo,
49    MixerOutputType::Aux,
50    MixerOutputType::Motor,
51    MixerOutputType::Aux,
52    MixerOutputType::Aux,
53    MixerOutputType::Aux,
54    MixerOutputType::Aux,
55    MixerOutputType::Aux,
56];
57const ESC_CALIBRATION_PWM_RATES: [f64; NUM_MIXER_OUTPUTS] = [50.0; NUM_MIXER_OUTPUTS];
58const QUAD_X_PWM_RATES: [f64; NUM_MIXER_OUTPUTS] = [
59    490.0, 490.0, 490.0, 490.0, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0,
60];
61const HEX_PWM_RATES: [f64; NUM_MIXER_OUTPUTS] = [
62    490.0, 490.0, 490.0, 490.0, 490.0, 490.0, 490.0, 490.0, 50.0, 50.0,
63];
64const OCTO_PWM_RATES: [f64; NUM_MIXER_OUTPUTS] = [
65    490.0, 490.0, 490.0, 490.0, 490.0, 490.0, 490.0, 490.0, 50.0, 50.0,
66];
67const FIXEDWING_PWM_RATES: [f64; NUM_MIXER_OUTPUTS] = [50.0; NUM_MIXER_OUTPUTS];
68
69#[rustfmt::skip]
70const QUAD_X_MIXER: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
71    [ 0.0000,  0.0000,  0.0000,  0.0000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], // Fx
72    [ 0.0000,  0.0000,  0.0000,  0.0000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], // Fy
73    [-0.2500, -0.2500, -0.2500, -0.2500, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], // Fz
74    [-0.7071, -0.7071,  0.7071,  0.7071, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], // Qx
75    [ 0.7071, -0.7071, -0.7071,  0.7071, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], // Qy
76    [ 1.0000, -1.0000,  1.0000, -1.0000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], // Qz
77    [ 0.0000,  0.0000,  0.0000,  0.0000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
78    [ 0.0000,  0.0000,  0.0000,  0.0000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
79    [ 0.0000,  0.0000,  0.0000,  0.0000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
80    [ 0.0000,  0.0000,  0.0000,  0.0000, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
81];
82
83#[rustfmt::skip]
84const ESC_CALIBRATION_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
85    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
86    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
87    [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
88    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
89    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
90    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
91    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
92    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
93    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
94    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
95];
96
97#[rustfmt::skip]
98const QUAD_PLUS_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
99    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
100    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
101    [-0.25, -0.25, -0.25, -0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
102    [0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
103    [1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
104    [1.0, -1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
105    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
106    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
107    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
108    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
109];
110
111#[rustfmt::skip]
112const FIXEDWING_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
113    [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
114    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
115    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
116    [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
117    [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
118    [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
119    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
120    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
121    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
122    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
123];
124
125#[rustfmt::skip]
126const INVERTED_VTAIL_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
127    [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
128    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
129    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
130    [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
131    [0.0, -0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
132    [0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
133    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
134    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
135    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
136    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
137];
138
139#[rustfmt::skip]
140const HEX_PLUS_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
141    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
142    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
143    [-0.1667, -0.1667, -0.1667, -0.1667, -0.1667, -0.1667, 0.0, 0.0, 0.0, 0.0],
144    [0.0, -0.8660, -0.8660, 0.0, 0.8660, 0.8660, 0.0, 0.0, 0.0, 0.0],
145    [1.0, 0.5, -0.5, -1.0, -0.5, 0.5, 0.0, 0.0, 0.0, 0.0],
146    [1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0],
147    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
148    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
149    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
150    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
151];
152
153#[rustfmt::skip]
154const HEX_X_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
155    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
156    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
157    [-0.1667, -0.1667, -0.1667, -0.1667, -0.1667, -0.1667, 0.0, 0.0, 0.0, 0.0],
158    [-0.5, -1.0, -0.5, 0.5, 1.0, 0.5, 0.0, 0.0, 0.0, 0.0],
159    [0.8660, 0.0, -0.8660, -0.8660, 0.0, 0.8660, 0.0, 0.0, 0.0, 0.0],
160    [1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0],
161    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
162    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
163    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
164    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
165];
166
167#[rustfmt::skip]
168const OCTO_PLUS_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
169    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
170    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
171    [-0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, 0.0, 0.0],
172    [0.0, -0.7071, -1.0, -0.7071, 0.0, 0.7071, 1.0, 0.7071, 0.0, 0.0],
173    [1.0, 0.7071, 0.0, -0.7071, -1.0, -0.7071, 0.0, 0.7071, 0.0, 0.0],
174    [1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 0.0, 0.0],
175    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
176    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
177    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
178    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
179];
180
181#[rustfmt::skip]
182const OCTO_X_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
183    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
184    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
185    [-0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, 0.0, 0.0],
186    [-0.3827, -0.9239, -0.9239, -0.3827, 0.3827, 0.9239, 0.9239, 0.3827, 0.0, 0.0],
187    [0.9239, 0.3827, -0.3827, -0.9239, -0.9239, -0.3827, 0.3827, 0.9239, 0.0, 0.0],
188    [1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 0.0, 0.0],
189    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
190    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
191    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
192    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
193];
194
195#[rustfmt::skip]
196const Y6_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
197    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
198    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
199    [-0.1667, -0.1667, -0.1667, -0.1667, -0.1667, -0.1667, 0.0, 0.0, 0.0, 0.0],
200    [-0.8660, -0.8660, 0.0, 0.0, 0.8660, 0.8660, 0.0, 0.0, 0.0, 0.0],
201    [0.5, 0.5, -1.0, -1.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0],
202    [1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 0.0, 0.0, 0.0, 0.0],
203    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
204    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
205    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
206    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
207];
208
209#[rustfmt::skip]
210const X8_MATRIX: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] = [
211    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
212    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
213    [-0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, -0.1250, 0.0, 0.0],
214    [-0.7071, -0.7071, -0.7071, -0.7071, 0.7071, 0.7071, 0.7071, 0.7071, 0.0, 0.0],
215    [0.7071, 0.7071, -0.7071, -0.7071, -0.7071, -0.7071, 0.7071, 0.7071, 0.0, 0.0],
216    [1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 0.0, 0.0],
217    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
218    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
219    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
220    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
221];
222
223#[derive(Debug, Clone, Copy)]
224pub struct MixerParams<R: FlightFloat> {
225    // Safety / limits
226    pub idle_throttle: R,
227    pub num_motors: usize,
228    pub fixed_wing: bool,
229    pub use_motor_parameters: bool,
230    pub throttle_axis: usize,
231}
232
233pub struct MatrixMixer<R: FlightFloat> {
234    params: MixerParams<R>,
235    status: MixerStatus,
236    output_types: [MixerOutputType; NUM_MIXER_OUTPUTS],
237    default_pwm_rates: [R; NUM_MIXER_OUTPUTS],
238    primary_mixer: [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS],
239    secondary_mixer: [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS],
240}
241
242#[derive(Clone, Copy)]
243struct CannedMixerConfig<R: FlightFloat> {
244    output_types: [MixerOutputType; NUM_MIXER_OUTPUTS],
245    default_pwm_rates: [R; NUM_MIXER_OUTPUTS],
246    matrix: [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS],
247}
248
249impl<R: FlightFloat> MatrixMixer<R> {
250    pub fn new(params: &Params) -> Self {
251        let mixer_params = MixerParams {
252            num_motors: 4,
253            idle_throttle: if let ParamValue::Float(v) =
254                params.get_by_id(ParamId::PARAM_MOTOR_IDLE_THROTTLE)
255            {
256                <R as FlightFloat>::from_f32(v)
257            } else {
258                <R as FlightFloat>::from_f32(0.1)
259            },
260            fixed_wing: false,
261            use_motor_parameters: false,
262            throttle_axis: 2,
263        };
264
265        let mut mixer = Self {
266            params: mixer_params,
267            status: MixerStatus::Healthy,
268            output_types: QUAD_X_OUTPUT_TYPES,
269            default_pwm_rates: [<R as FlightFloat>::from_f32(50.0); NUM_MIXER_OUTPUTS],
270            primary_mixer: matrix_from_default(QUAD_X_MIXER),
271            secondary_mixer: matrix_from_default(QUAD_X_MIXER),
272        };
273        mixer.status = mixer.refresh_mixer_config(params);
274        mixer.refresh_runtime_params(params);
275        mixer
276    }
277}
278
279impl<R: FlightFloat> Mixer<R> for MatrixMixer<R> {
280    type MixerInput = ControllerOutput<R>;
281    type ActuatorCommands = [R; NUM_MIXER_OUTPUTS];
282
283    fn mix(
284        &mut self,
285        controls: &Self::MixerInput,
286        ctx: MixerCtx<'_, R>,
287    ) -> MixerRun<Self::ActuatorCommands> {
288        let status = self.status;
289
290        if matches!(status, MixerStatus::InvalidMixer) {
291            return MixerRun {
292                commands: [<R as FlightFloat>::from_f32(0.0); NUM_MIXER_OUTPUTS],
293                status,
294            };
295        }
296
297        let mut commands = *controls;
298        if self.params.fixed_wing {
299            apply_fixedwing_reversals(&mut commands, ctx.params);
300        } else if throttle_command(&commands, self.params.throttle_axis).abs()
301            < self.params.idle_throttle
302        {
303            commands.u[5] = <R as FlightFloat>::from_f32(0.0);
304        }
305
306        let mut outputs = [<R as FlightFloat>::from_f32(0.0); NUM_MIXER_OUTPUTS];
307        let mut max_output = <R as FlightFloat>::from_f32(1.0);
308
309        if self.params.use_motor_parameters {
310            for output in 0..NUM_MIXER_OUTPUTS {
311                if self.output_types[output] == MixerOutputType::Aux {
312                    continue;
313                }
314
315                let value = if self.output_types[output] == MixerOutputType::Motor {
316                    self.mix_motor_parameter_output(output, &commands, ctx.rc_override, &ctx)
317                } else {
318                    self.mix_single_output_with_selected_rows(output, &commands, ctx.rc_override)
319                };
320                outputs[output] = value;
321
322                if self.output_types[output] == MixerOutputType::Motor && value.abs() > max_output {
323                    max_output = value.abs();
324                }
325            }
326        } else {
327            self.mix_outputs_with_selected_rows(&commands, ctx.rc_override, &mut outputs);
328            for (output, value) in outputs.iter().enumerate() {
329                if self.output_types[output] == MixerOutputType::Motor && value.abs() > max_output {
330                    max_output = value.abs();
331                }
332            }
333        }
334
335        if max_output > <R as FlightFloat>::from_f32(2.0) {
336            crate::log_warn!("Output from mixer is {}! Check mixer", max_output);
337        }
338
339        let scale_factor = if max_output > <R as FlightFloat>::from_f32(1.0) {
340            <R as FlightFloat>::from_f32(1.0) / max_output
341        } else {
342            <R as FlightFloat>::from_f32(1.0)
343        };
344
345        for (output, value) in outputs.iter_mut().enumerate() {
346            if self.output_types[output] != MixerOutputType::Motor {
347                continue;
348            }
349
350            *value *= scale_factor;
351        }
352
353        MixerRun {
354            commands: outputs,
355            status,
356        }
357    }
358
359    fn output_types(&self) -> &[MixerOutputType] {
360        &self.output_types
361    }
362
363    fn default_pwm_rates(&self) -> &[R] {
364        &self.default_pwm_rates
365    }
366
367    fn on_param_changed(&mut self, params: &Params, id: ParamId) -> Option<MixerStatus> {
368        if is_mixer_runtime_param(id) {
369            self.refresh_runtime_params(params);
370        }
371        if is_mixer_config_param(id) {
372            self.status = self.refresh_mixer_config(params);
373            self.refresh_runtime_params(params);
374            Some(self.status)
375        } else {
376            None
377        }
378    }
379
380    fn refresh_params(&mut self, params: &Params) -> Option<MixerStatus> {
381        self.status = self.refresh_mixer_config(params);
382        self.refresh_runtime_params(params);
383        Some(self.status)
384    }
385}
386
387impl<R: FlightFloat> MatrixMixer<R> {
388    fn refresh_runtime_params(&mut self, params: &Params) {
389        self.params.idle_throttle = param_float(params, ParamId::PARAM_MOTOR_IDLE_THROTTLE);
390        self.params.num_motors = param_int(params, ParamId::PARAM_NUM_MOTORS).max(0) as usize;
391        let primary_choice = param_int(params, ParamId::PARAM_PRIMARY_MIXER);
392        self.params.fixed_wing = param_int(params, ParamId::PARAM_FIXED_WING) != 0
393            || matches!(primary_choice, FIXEDWING_MIXER | INVERTED_VTAIL_MIXER);
394        self.params.use_motor_parameters =
395            !self.params.fixed_wing && param_int(params, ParamId::PARAM_USE_MOTOR_PARAMETERS) != 0;
396        self.params.throttle_axis =
397            param_int(params, ParamId::PARAM_RC_F_AXIS).clamp(0, 2) as usize;
398    }
399
400    fn refresh_mixer_config(&mut self, params: &Params) -> MixerStatus {
401        let primary_choice = param_int(params, ParamId::PARAM_PRIMARY_MIXER);
402        if primary_choice >= NUM_MIXERS {
403            crate::log_error!("Invalid mixer choice for primary mixer");
404            return MixerStatus::InvalidMixer;
405        }
406
407        if let Some(config) = canned_mixer(primary_choice) {
408            self.output_types = config.output_types;
409            self.default_pwm_rates = config.default_pwm_rates;
410            self.primary_mixer = config.matrix;
411        } else {
412            self.output_types = output_types_from_params(params).unwrap_or(QUAD_X_OUTPUT_TYPES);
413            self.default_pwm_rates = pwm_rates_from_params(params)
414                .unwrap_or([<R as FlightFloat>::from_f32(50.0); NUM_MIXER_OUTPUTS]);
415            self.primary_mixer = mixer_from_params(
416                params,
417                ParamId::PARAM_PRIMARY_MIXER,
418                ParamId::PARAM_PRIMARY_MIXER_0_0,
419            )
420            .unwrap_or(matrix_from_default(QUAD_X_MIXER));
421        }
422
423        let secondary_choice = param_int(params, ParamId::PARAM_SECONDARY_MIXER);
424        self.secondary_mixer = if let Some(config) = canned_secondary_mixer(secondary_choice) {
425            config.matrix
426        } else {
427            mixer_from_params(
428                params,
429                ParamId::PARAM_SECONDARY_MIXER,
430                ParamId::PARAM_SECONDARY_MIXER_0_0,
431            )
432            .unwrap_or(self.primary_mixer)
433        };
434
435        MixerStatus::Healthy
436    }
437
438    #[cfg(test)]
439    fn select_primary_or_secondary(
440        &self,
441        rc_override: u16,
442    ) -> [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] {
443        let mut mixer = self.secondary_mixer;
444
445        if rc_override & ATTITUDE_OVERRIDDEN != 0 {
446            mixer[3] = self.primary_mixer[3];
447            mixer[4] = self.primary_mixer[4];
448            mixer[5] = self.primary_mixer[5];
449        }
450
451        if rc_override & T_OVERRIDDEN != 0 {
452            mixer[0] = self.primary_mixer[0];
453            mixer[1] = self.primary_mixer[1];
454            mixer[2] = self.primary_mixer[2];
455        }
456
457        mixer
458    }
459
460    fn mix_motor_parameter_output(
461        &self,
462        output: usize,
463        commands: &ControllerOutput<R>,
464        rc_override: u16,
465        ctx: &MixerCtx<'_, R>,
466    ) -> R {
467        let omega_squared = self
468            .mix_single_output_with_selected_rows(output, commands, rc_override)
469            .max(<R as FlightFloat>::from_f32(0.0));
470        let k_q = param_float(ctx.params, ParamId::PARAM_MOTOR_KV);
471        if k_q < <R as FlightFloat>::from_f32(0.0000001) {
472            return <R as FlightFloat>::from_f32(0.0);
473        }
474
475        let Some(battery_voltage) = ctx.battery_voltage else {
476            return <R as FlightFloat>::from_f32(0.0);
477        };
478        if battery_voltage < <R as FlightFloat>::from_f32(0.0001) {
479            return <R as FlightFloat>::from_f32(0.0);
480        }
481
482        let resistance = param_float::<R>(ctx.params, ParamId::PARAM_MOTOR_RESISTANCE);
483        let diameter = param_float::<R>(ctx.params, ParamId::PARAM_PROP_DIAMETER);
484        let cq = param_float::<R>(ctx.params, ParamId::PARAM_PROP_CQ);
485        let kv = param_float::<R>(ctx.params, ParamId::PARAM_MOTOR_KV);
486        let no_load_current = param_float::<R>(ctx.params, ParamId::PARAM_NO_LOAD_CURRENT);
487
488        let diameter_5 = diameter * diameter * diameter * diameter * diameter;
489        let pi = pi::<R>();
490        let pi_2 = pi * pi;
491        let voltage = ctx.air_density * diameter_5 / (<R as FlightFloat>::from_f32(4.0) * pi_2)
492            * omega_squared
493            * cq
494            * resistance
495            / k_q
496            + resistance * no_load_current
497            + kv * omega_squared.sqrt();
498
499        voltage / battery_voltage
500    }
501
502    fn mix_single_output_with_selected_rows(
503        &self,
504        output: usize,
505        commands: &ControllerOutput<R>,
506        rc_override: u16,
507    ) -> R {
508        let mut value = <R as FlightFloat>::from_f32(0.0);
509        for input in 0..NUM_MIXER_OUTPUTS {
510            let command = commands.u[input];
511            if command == <R as FlightFloat>::from_f32(0.0) {
512                continue;
513            }
514            let row = self.selected_mixer_row(input, rc_override);
515            value += command * row[output];
516        }
517        value
518    }
519
520    fn mix_outputs_with_selected_rows(
521        &self,
522        commands: &ControllerOutput<R>,
523        rc_override: u16,
524        outputs: &mut [R; NUM_MIXER_OUTPUTS],
525    ) {
526        for input in 0..NUM_MIXER_OUTPUTS {
527            let command = commands.u[input];
528            if command == <R as FlightFloat>::from_f32(0.0) {
529                continue;
530            }
531            let row = self.selected_mixer_row(input, rc_override);
532            for output in 0..NUM_MIXER_OUTPUTS {
533                if self.output_types[output] != MixerOutputType::Aux {
534                    outputs[output] += command * row[output];
535                }
536            }
537        }
538    }
539
540    #[inline(always)]
541    fn selected_mixer_row(&self, input: usize, rc_override: u16) -> &[R; NUM_MIXER_OUTPUTS] {
542        if self.use_primary_row_for_override(input, rc_override) {
543            &self.primary_mixer[input]
544        } else {
545            &self.secondary_mixer[input]
546        }
547    }
548
549    fn use_primary_row_for_override(&self, input: usize, rc_override: u16) -> bool {
550        ((3..=5).contains(&input) && rc_override & ATTITUDE_OVERRIDDEN != 0)
551            || (input <= 2 && rc_override & T_OVERRIDDEN != 0)
552    }
553}
554
555fn throttle_command<R: FlightFloat>(commands: &ControllerOutput<R>, throttle_axis: usize) -> R {
556    commands.u[throttle_axis.min(2)]
557}
558
559fn param_float<R: FlightFloat>(params: &Params, id: ParamId) -> R {
560    match params.get_by_id(id) {
561        ParamValue::Float(value) => <R as FlightFloat>::from_f32(value),
562        _ => <R as FlightFloat>::from_f32(0.0),
563    }
564}
565
566fn param_int(params: &Params, id: ParamId) -> i32 {
567    match params.get_by_id(id) {
568        ParamValue::Int(value) => value,
569        _ => 0,
570    }
571}
572
573fn param_in_range(id: ParamId, first: ParamId, last: ParamId) -> bool {
574    let id = id as usize;
575    id >= first as usize && id <= last as usize
576}
577
578fn is_mixer_config_param(id: ParamId) -> bool {
579    matches!(
580        id,
581        ParamId::PARAM_PRIMARY_MIXER | ParamId::PARAM_SECONDARY_MIXER
582    ) || param_in_range(
583        id,
584        ParamId::PARAM_PRIMARY_MIXER_OUTPUT_0,
585        ParamId::PARAM_PRIMARY_MIXER_OUTPUT_9,
586    ) || param_in_range(
587        id,
588        ParamId::PARAM_PRIMARY_MIXER_PWM_RATE_0,
589        ParamId::PARAM_PRIMARY_MIXER_PWM_RATE_9,
590    ) || param_in_range(
591        id,
592        ParamId::PARAM_PRIMARY_MIXER_0_0,
593        ParamId::PARAM_PRIMARY_MIXER_9_9,
594    ) || param_in_range(
595        id,
596        ParamId::PARAM_SECONDARY_MIXER_0_0,
597        ParamId::PARAM_SECONDARY_MIXER_9_9,
598    )
599}
600
601fn is_mixer_runtime_param(id: ParamId) -> bool {
602    matches!(
603        id,
604        ParamId::PARAM_MOTOR_IDLE_THROTTLE
605            | ParamId::PARAM_NUM_MOTORS
606            | ParamId::PARAM_FIXED_WING
607            | ParamId::PARAM_USE_MOTOR_PARAMETERS
608            | ParamId::PARAM_RC_F_AXIS
609    )
610}
611
612fn mixer_from_params<R: FlightFloat>(
613    params: &Params,
614    mixer_choice_id: ParamId,
615    first_matrix_id: ParamId,
616) -> Option<[[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS]> {
617    if param_int(params, mixer_choice_id) != CUSTOM_MIXER {
618        return None;
619    }
620
621    let mut mixer = [[<R as FlightFloat>::from_f32(0.0); NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS];
622    let first_index = first_matrix_id as usize;
623    for col in 0..NUM_MIXER_OUTPUTS {
624        for (row, mixer_row) in mixer.iter_mut().enumerate() {
625            let param_index = first_index + col * NUM_MIXER_OUTPUTS + row;
626            let Some(param_id) = ParamId::from_index(param_index) else {
627                return None;
628            };
629            mixer_row[col] = param_float(params, param_id);
630        }
631    }
632
633    Some(mixer)
634}
635
636fn output_types_from_params(params: &Params) -> Option<[MixerOutputType; NUM_MIXER_OUTPUTS]> {
637    if param_int(params, ParamId::PARAM_PRIMARY_MIXER) != CUSTOM_MIXER {
638        return None;
639    }
640
641    let mut output_types = [MixerOutputType::Aux; NUM_MIXER_OUTPUTS];
642    let first_index = ParamId::PARAM_PRIMARY_MIXER_OUTPUT_0 as usize;
643    for (output, output_type) in output_types.iter_mut().enumerate() {
644        let param_id = ParamId::from_index(first_index + output)?;
645        *output_type = mixer_output_type_from_rosflight(param_int(params, param_id));
646    }
647
648    Some(output_types)
649}
650
651fn pwm_rates_from_params<R: FlightFloat>(params: &Params) -> Option<[R; NUM_MIXER_OUTPUTS]> {
652    if param_int(params, ParamId::PARAM_PRIMARY_MIXER) != CUSTOM_MIXER {
653        return None;
654    }
655
656    let mut rates = [<R as FlightFloat>::from_f32(0.0); NUM_MIXER_OUTPUTS];
657    let first_index = ParamId::PARAM_PRIMARY_MIXER_PWM_RATE_0 as usize;
658    for (output, rate) in rates.iter_mut().enumerate() {
659        let param_id = ParamId::from_index(first_index + output)?;
660        *rate = param_float(params, param_id);
661    }
662
663    Some(rates)
664}
665
666fn mixer_output_type_from_rosflight(value: i32) -> MixerOutputType {
667    match value {
668        1 => MixerOutputType::Servo,
669        2 => MixerOutputType::Motor,
670        3 => MixerOutputType::Gpio,
671        _ => MixerOutputType::Aux,
672    }
673}
674
675fn mixer_output_type_to_rosflight(value: MixerOutputType) -> i32 {
676    match value {
677        MixerOutputType::Aux => 0,
678        MixerOutputType::Servo => 1,
679        MixerOutputType::Motor => 2,
680        MixerOutputType::Gpio => 3,
681    }
682}
683
684pub fn sync_reflected_mixer_params(params: &mut Params, changed: ParamId) {
685    match changed {
686        ParamId::PARAM_PRIMARY_MIXER => {
687            let choice = param_int(params, ParamId::PARAM_PRIMARY_MIXER);
688            let Some(config) = canned_mixer::<f64>(choice) else {
689                return;
690            };
691            save_primary_mixer_params(params, config);
692            let secondary_choice = param_int(params, ParamId::PARAM_SECONDARY_MIXER);
693            if !(0..NUM_MIXERS).contains(&secondary_choice) {
694                save_secondary_mixer_params(params, config.matrix);
695            }
696        }
697        ParamId::PARAM_SECONDARY_MIXER => {
698            let choice = param_int(params, ParamId::PARAM_SECONDARY_MIXER);
699            let Some(config) = canned_secondary_mixer::<f64>(choice) else {
700                return;
701            };
702            save_secondary_mixer_params(params, config.matrix);
703        }
704        _ => {}
705    }
706}
707
708fn save_primary_mixer_params(params: &mut Params, config: CannedMixerConfig<f64>) {
709    for output in 0..NUM_MIXER_OUTPUTS {
710        if let Some(output_id) =
711            ParamId::from_index(ParamId::PARAM_PRIMARY_MIXER_OUTPUT_0 as usize + output)
712        {
713            params.set_by_id(
714                output_id,
715                ParamValue::Int(mixer_output_type_to_rosflight(config.output_types[output])),
716            );
717        }
718
719        if let Some(rate_id) =
720            ParamId::from_index(ParamId::PARAM_PRIMARY_MIXER_PWM_RATE_0 as usize + output)
721        {
722            params.set_by_id(
723                rate_id,
724                ParamValue::Float(config.default_pwm_rates[output].to_f32_lossy()),
725            );
726        }
727    }
728
729    save_mixer_matrix_params(params, ParamId::PARAM_PRIMARY_MIXER_0_0, config.matrix);
730}
731
732fn save_secondary_mixer_params(
733    params: &mut Params,
734    matrix: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS],
735) {
736    save_mixer_matrix_params(params, ParamId::PARAM_SECONDARY_MIXER_0_0, matrix);
737}
738
739fn save_mixer_matrix_params(
740    params: &mut Params,
741    first_matrix_id: ParamId,
742    matrix: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS],
743) {
744    let first_index = first_matrix_id as usize;
745    for col in 0..NUM_MIXER_OUTPUTS {
746        for (row, mixer_row) in matrix.iter().enumerate() {
747            let Some(param_id) = ParamId::from_index(first_index + col * NUM_MIXER_OUTPUTS + row)
748            else {
749                return;
750            };
751            params.set_by_id(param_id, ParamValue::Float(mixer_row[col].to_f32_lossy()));
752        }
753    }
754}
755
756fn matrix_from_default<R: FlightFloat>(
757    matrix: [[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS],
758) -> [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] {
759    matrix.map(|row| row.map(<R as FlightFloat>::from_f64))
760}
761
762fn rates_from_default<R: FlightFloat>(rates: [f64; NUM_MIXER_OUTPUTS]) -> [R; NUM_MIXER_OUTPUTS] {
763    rates.map(<R as FlightFloat>::from_f64)
764}
765
766fn inverted_multirotor_mixer<R: FlightFloat>(
767    mixer: [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS],
768) -> [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] {
769    let mut mixer_matrix = SMatrix::<R, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS>::zeros();
770    for row in 0..NUM_MIXER_OUTPUTS {
771        for col in 0..NUM_MIXER_OUTPUTS {
772            mixer_matrix[(row, col)] = mixer[row][col];
773        }
774    }
775
776    let svd = mixer_matrix.svd(true, true);
777    let Some(u) = svd.u else {
778        return mixer;
779    };
780    let Some(v_t) = svd.v_t else {
781        return mixer;
782    };
783
784    let mut sigma_inverse = SMatrix::<R, NUM_MIXER_OUTPUTS, NUM_MIXER_OUTPUTS>::zeros();
785    for i in 0..NUM_MIXER_OUTPUTS {
786        let singular_value = svd.singular_values[i];
787        if singular_value != <R as FlightFloat>::from_f32(0.0) {
788            sigma_inverse[(i, i)] = <R as FlightFloat>::from_f32(1.0) / singular_value;
789        }
790    }
791
792    let mixer_matrix_pinv = v_t.transpose() * sigma_inverse * u.transpose();
793    let mut inverted = [[<R as FlightFloat>::from_f32(0.0); NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS];
794    for i in 0..NUM_MIXER_OUTPUTS {
795        for j in 0..NUM_MIXER_OUTPUTS {
796            inverted[j][i] = mixer_matrix_pinv[(i, j)];
797        }
798    }
799    inverted
800}
801
802fn rank_one_pseudoinverse_mixer<R: FlightFloat>(
803    mixer: [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS],
804) -> [[R; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS] {
805    let mut norm_squared = <R as FlightFloat>::from_f32(0.0);
806    for row in mixer.iter() {
807        for value in row.iter() {
808            norm_squared += *value * *value;
809        }
810    }
811
812    if norm_squared < <R as FlightFloat>::from_f64(1.0e-12) {
813        return mixer;
814    }
815
816    let mut inverted = [[<R as FlightFloat>::from_f32(0.0); NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS];
817    for row in 0..NUM_MIXER_OUTPUTS {
818        for col in 0..NUM_MIXER_OUTPUTS {
819            inverted[row][col] = mixer[row][col] / norm_squared;
820        }
821    }
822
823    inverted
824}
825
826fn canned_mixer<R: FlightFloat>(choice: i32) -> Option<CannedMixerConfig<R>> {
827    let config = match choice {
828        ESC_CALIBRATION_MIXER => CannedMixerConfig {
829            output_types: ALL_MOTOR_OUTPUT_TYPES,
830            default_pwm_rates: rates_from_default(ESC_CALIBRATION_PWM_RATES),
831            matrix: rank_one_pseudoinverse_mixer(matrix_from_default(ESC_CALIBRATION_MATRIX)),
832        },
833        QUAD_PLUS_MIXER => CannedMixerConfig {
834            output_types: QUAD_X_OUTPUT_TYPES,
835            default_pwm_rates: rates_from_default(QUAD_X_PWM_RATES),
836            matrix: inverted_multirotor_mixer(matrix_from_default(QUAD_PLUS_MATRIX)),
837        },
838        QUAD_X_MIXER_CHOICE => CannedMixerConfig {
839            output_types: QUAD_X_OUTPUT_TYPES,
840            default_pwm_rates: rates_from_default(QUAD_X_PWM_RATES),
841            matrix: inverted_multirotor_mixer(matrix_from_default(QUAD_X_MIXER)),
842        },
843        HEX_PLUS_MIXER => CannedMixerConfig {
844            output_types: [
845                MixerOutputType::Motor,
846                MixerOutputType::Motor,
847                MixerOutputType::Motor,
848                MixerOutputType::Motor,
849                MixerOutputType::Motor,
850                MixerOutputType::Motor,
851                MixerOutputType::Aux,
852                MixerOutputType::Aux,
853                MixerOutputType::Aux,
854                MixerOutputType::Aux,
855            ],
856            default_pwm_rates: rates_from_default(HEX_PWM_RATES),
857            matrix: inverted_multirotor_mixer(matrix_from_default(HEX_PLUS_MATRIX)),
858        },
859        HEX_X_MIXER => CannedMixerConfig {
860            output_types: [
861                MixerOutputType::Motor,
862                MixerOutputType::Motor,
863                MixerOutputType::Motor,
864                MixerOutputType::Motor,
865                MixerOutputType::Motor,
866                MixerOutputType::Motor,
867                MixerOutputType::Aux,
868                MixerOutputType::Aux,
869                MixerOutputType::Aux,
870                MixerOutputType::Aux,
871            ],
872            default_pwm_rates: rates_from_default(HEX_PWM_RATES),
873            matrix: inverted_multirotor_mixer(matrix_from_default(HEX_X_MATRIX)),
874        },
875        Y6_MIXER => CannedMixerConfig {
876            output_types: [
877                MixerOutputType::Motor,
878                MixerOutputType::Motor,
879                MixerOutputType::Motor,
880                MixerOutputType::Motor,
881                MixerOutputType::Motor,
882                MixerOutputType::Motor,
883                MixerOutputType::Aux,
884                MixerOutputType::Aux,
885                MixerOutputType::Aux,
886                MixerOutputType::Aux,
887            ],
888            default_pwm_rates: rates_from_default(HEX_PWM_RATES),
889            matrix: inverted_multirotor_mixer(matrix_from_default(Y6_MATRIX)),
890        },
891        OCTO_PLUS_MIXER => CannedMixerConfig {
892            output_types: [
893                MixerOutputType::Motor,
894                MixerOutputType::Motor,
895                MixerOutputType::Motor,
896                MixerOutputType::Motor,
897                MixerOutputType::Motor,
898                MixerOutputType::Motor,
899                MixerOutputType::Motor,
900                MixerOutputType::Motor,
901                MixerOutputType::Aux,
902                MixerOutputType::Aux,
903            ],
904            default_pwm_rates: rates_from_default(OCTO_PWM_RATES),
905            matrix: inverted_multirotor_mixer(matrix_from_default(OCTO_PLUS_MATRIX)),
906        },
907        OCTO_X_MIXER => CannedMixerConfig {
908            output_types: [
909                MixerOutputType::Motor,
910                MixerOutputType::Motor,
911                MixerOutputType::Motor,
912                MixerOutputType::Motor,
913                MixerOutputType::Motor,
914                MixerOutputType::Motor,
915                MixerOutputType::Motor,
916                MixerOutputType::Motor,
917                MixerOutputType::Aux,
918                MixerOutputType::Aux,
919            ],
920            default_pwm_rates: rates_from_default(OCTO_PWM_RATES),
921            matrix: inverted_multirotor_mixer(matrix_from_default(OCTO_X_MATRIX)),
922        },
923        X8_MIXER => CannedMixerConfig {
924            output_types: [
925                MixerOutputType::Motor,
926                MixerOutputType::Motor,
927                MixerOutputType::Motor,
928                MixerOutputType::Motor,
929                MixerOutputType::Motor,
930                MixerOutputType::Motor,
931                MixerOutputType::Motor,
932                MixerOutputType::Motor,
933                MixerOutputType::Aux,
934                MixerOutputType::Aux,
935            ],
936            default_pwm_rates: rates_from_default(OCTO_PWM_RATES),
937            matrix: inverted_multirotor_mixer(matrix_from_default(X8_MATRIX)),
938        },
939        FIXEDWING_MIXER => CannedMixerConfig {
940            output_types: FIXEDWING_OUTPUT_TYPES,
941            default_pwm_rates: rates_from_default(FIXEDWING_PWM_RATES),
942            matrix: matrix_from_default(FIXEDWING_MATRIX),
943        },
944        INVERTED_VTAIL_MIXER => CannedMixerConfig {
945            output_types: FIXEDWING_OUTPUT_TYPES,
946            default_pwm_rates: rates_from_default(FIXEDWING_PWM_RATES),
947            matrix: matrix_from_default(INVERTED_VTAIL_MATRIX),
948        },
949        _ => return None,
950    };
951
952    Some(config)
953}
954
955fn canned_secondary_mixer<R: FlightFloat>(choice: i32) -> Option<CannedMixerConfig<R>> {
956    let mut config = canned_mixer(choice)?;
957    config.matrix = match choice {
958        CUSTOM_MIXER => config.matrix,
959        _ => inverted_multirotor_mixer(matrix_from_default(raw_canned_mixer_matrix(choice)?)),
960    };
961    Some(config)
962}
963
964fn raw_canned_mixer_matrix(choice: i32) -> Option<[[f64; NUM_MIXER_OUTPUTS]; NUM_MIXER_OUTPUTS]> {
965    match choice {
966        ESC_CALIBRATION_MIXER => Some(ESC_CALIBRATION_MATRIX),
967        QUAD_PLUS_MIXER => Some(QUAD_PLUS_MATRIX),
968        QUAD_X_MIXER_CHOICE => Some(QUAD_X_MIXER),
969        HEX_PLUS_MIXER => Some(HEX_PLUS_MATRIX),
970        HEX_X_MIXER => Some(HEX_X_MATRIX),
971        OCTO_PLUS_MIXER => Some(OCTO_PLUS_MATRIX),
972        OCTO_X_MIXER => Some(OCTO_X_MATRIX),
973        Y6_MIXER => Some(Y6_MATRIX),
974        X8_MIXER => Some(X8_MATRIX),
975        FIXEDWING_MIXER => Some(FIXEDWING_MATRIX),
976        INVERTED_VTAIL_MIXER => Some(INVERTED_VTAIL_MATRIX),
977        _ => None,
978    }
979}
980
981fn apply_fixedwing_reversals<R: FlightFloat>(commands: &mut ControllerOutput<R>, params: &Params) {
982    if param_int(params, ParamId::PARAM_AILERON_REVERSE) != 0 {
983        commands.u[3] *= <R as FlightFloat>::from_f32(-1.0);
984    }
985    if param_int(params, ParamId::PARAM_ELEVATOR_REVERSE) != 0 {
986        commands.u[4] *= <R as FlightFloat>::from_f32(-1.0);
987    }
988    if param_int(params, ParamId::PARAM_RUDDER_REVERSE) != 0 {
989        commands.u[5] *= <R as FlightFloat>::from_f32(-1.0);
990    }
991}
992
993#[cfg(test)]
994mod tests {
995    use super::*;
996    use crate::command::{OVERRIDE_ATT_SWITCH, OVERRIDE_NO_OVERRIDE, OVERRIDE_THR_SWITCH};
997    use crate::state_machine::StateManager;
998
999    fn mixer_ctx<'a>(
1000        state: &'a StateManager,
1001        params: &'a Params,
1002        rc_override: u16,
1003    ) -> MixerCtx<'a, f64> {
1004        MixerCtx {
1005            state,
1006            params,
1007            rc_override,
1008            air_density: 1.225,
1009            battery_voltage: None,
1010        }
1011    }
1012
1013    #[test]
1014    fn canned_fixedwing_mixer_maps_controls_and_pwm_rates_like_rosflight_c() {
1015        let mut params = Params::new();
1016        params.set_by_id(ParamId::PARAM_FIXED_WING, ParamValue::Int(1));
1017        params.set_by_id(
1018            ParamId::PARAM_PRIMARY_MIXER,
1019            ParamValue::Int(FIXEDWING_MIXER),
1020        );
1021        let state = StateManager::new();
1022        let mut mixer = MatrixMixer::<f64>::new(&params);
1023        let mut controls = ControllerOutput::<f64>::default();
1024        controls.u[0] = 0.7;
1025        controls.u[3] = 0.1;
1026        controls.u[4] = -0.2;
1027        controls.u[5] = 0.3;
1028
1029        let run = mixer.mix(&controls, mixer_ctx(&state, &params, OVERRIDE_NO_OVERRIDE));
1030
1031        assert_eq!(run.status, MixerStatus::Healthy);
1032        assert_eq!(
1033            <MatrixMixer<f64> as Mixer<f64>>::output_types(&mixer),
1034            [
1035                MixerOutputType::Servo,
1036                MixerOutputType::Servo,
1037                MixerOutputType::Servo,
1038                MixerOutputType::Aux,
1039                MixerOutputType::Motor,
1040                MixerOutputType::Aux,
1041                MixerOutputType::Aux,
1042                MixerOutputType::Aux,
1043                MixerOutputType::Aux,
1044                MixerOutputType::Aux,
1045            ]
1046        );
1047        assert_eq!(
1048            <MatrixMixer<f64> as Mixer<f64>>::default_pwm_rates(&mixer),
1049            [50.0; NUM_MIXER_OUTPUTS]
1050        );
1051        assert!((run.commands[0] - 0.1).abs() < 1e-9);
1052        assert!((run.commands[1] + 0.2).abs() < 1e-9);
1053        assert!((run.commands[2] - 0.3).abs() < 1e-9);
1054        assert_eq!(run.commands[3], 0.0);
1055        assert!((run.commands[4] - 0.7).abs() < 1e-9);
1056    }
1057
1058    #[test]
1059    fn canned_fixedwing_reversal_params_apply_before_mixing() {
1060        let mut params = Params::new();
1061        params.set_by_id(ParamId::PARAM_FIXED_WING, ParamValue::Int(1));
1062        params.set_by_id(
1063            ParamId::PARAM_PRIMARY_MIXER,
1064            ParamValue::Int(FIXEDWING_MIXER),
1065        );
1066        params.set_by_id(ParamId::PARAM_AILERON_REVERSE, ParamValue::Int(1));
1067        params.set_by_id(ParamId::PARAM_ELEVATOR_REVERSE, ParamValue::Int(1));
1068        params.set_by_id(ParamId::PARAM_RUDDER_REVERSE, ParamValue::Int(1));
1069        let state = StateManager::new();
1070        let mut mixer = MatrixMixer::<f64>::new(&params);
1071        let mut controls = ControllerOutput::<f64>::default();
1072        controls.u[3] = 0.1;
1073        controls.u[4] = -0.2;
1074        controls.u[5] = 0.3;
1075
1076        let run = mixer.mix(&controls, mixer_ctx(&state, &params, OVERRIDE_NO_OVERRIDE));
1077
1078        assert!((run.commands[0] + 0.1).abs() < 1e-9);
1079        assert!((run.commands[1] - 0.2).abs() < 1e-9);
1080        assert!((run.commands[2] + 0.3).abs() < 1e-9);
1081    }
1082
1083    #[test]
1084    fn secondary_mixer_row_selection_matches_rosflight_c_override_masks() {
1085        let mut params = Params::new();
1086        params.set_by_id(
1087            ParamId::PARAM_PRIMARY_MIXER,
1088            ParamValue::Int(FIXEDWING_MIXER),
1089        );
1090        params.set_by_id(
1091            ParamId::PARAM_SECONDARY_MIXER,
1092            ParamValue::Int(INVERTED_VTAIL_MIXER),
1093        );
1094        let mut mixer = MatrixMixer::<f64>::new(&params);
1095        assert_eq!(mixer.refresh_mixer_config(&params), MixerStatus::Healthy);
1096
1097        let secondary_only = mixer.select_primary_or_secondary(OVERRIDE_NO_OVERRIDE);
1098        assert_eq!(secondary_only[0], mixer.secondary_mixer[0]);
1099        assert_eq!(secondary_only[3], mixer.secondary_mixer[3]);
1100
1101        let attitude_override = mixer.select_primary_or_secondary(OVERRIDE_ATT_SWITCH);
1102        assert_eq!(attitude_override[0], mixer.secondary_mixer[0]);
1103        assert_eq!(attitude_override[1], mixer.secondary_mixer[1]);
1104        assert_eq!(attitude_override[2], mixer.secondary_mixer[2]);
1105        assert_eq!(attitude_override[3], mixer.primary_mixer[3]);
1106        assert_eq!(attitude_override[4], mixer.primary_mixer[4]);
1107        assert_eq!(attitude_override[5], mixer.primary_mixer[5]);
1108
1109        let throttle_override = mixer.select_primary_or_secondary(OVERRIDE_THR_SWITCH);
1110        assert_eq!(throttle_override[0], mixer.primary_mixer[0]);
1111        assert_eq!(throttle_override[1], mixer.primary_mixer[1]);
1112        assert_eq!(throttle_override[2], mixer.primary_mixer[2]);
1113        assert_eq!(throttle_override[3], mixer.secondary_mixer[3]);
1114        assert_eq!(throttle_override[4], mixer.secondary_mixer[4]);
1115        assert_eq!(throttle_override[5], mixer.secondary_mixer[5]);
1116    }
1117
1118    #[test]
1119    fn reflected_secondary_mixer_defaults_to_primary_when_choice_is_unset() {
1120        let mut params = Params::new();
1121        params.set_by_id(
1122            ParamId::PARAM_PRIMARY_MIXER,
1123            ParamValue::Int(INVERTED_VTAIL_MIXER),
1124        );
1125        params.set_by_id(ParamId::PARAM_SECONDARY_MIXER, ParamValue::Int(-1));
1126
1127        sync_reflected_mixer_params(&mut params, ParamId::PARAM_PRIMARY_MIXER);
1128
1129        assert_eq!(
1130            params.get_by_id(ParamId::PARAM_PRIMARY_MIXER_3_0),
1131            params.get_by_id(ParamId::PARAM_SECONDARY_MIXER_3_0)
1132        );
1133        assert_eq!(
1134            params.get_by_id(ParamId::PARAM_PRIMARY_MIXER_4_1),
1135            params.get_by_id(ParamId::PARAM_SECONDARY_MIXER_4_1)
1136        );
1137        assert_eq!(
1138            params.get_by_id(ParamId::PARAM_PRIMARY_MIXER_5_1),
1139            params.get_by_id(ParamId::PARAM_SECONDARY_MIXER_5_1)
1140        );
1141    }
1142
1143    #[test]
1144    fn custom_motor_mixer_treats_negative_ned_fz_as_positive_thrust() {
1145        let mut params = Params::new();
1146        params.set_by_id(ParamId::PARAM_PRIMARY_MIXER, ParamValue::Int(CUSTOM_MIXER));
1147        params.set_by_id(ParamId::PARAM_SECONDARY_MIXER, ParamValue::Int(-1));
1148        params.set_by_id(ParamId::PARAM_USE_MOTOR_PARAMETERS, ParamValue::Int(1));
1149        params.set_by_id(ParamId::PARAM_NUM_MOTORS, ParamValue::Int(4));
1150        params.set_by_id(ParamId::PARAM_MOTOR_RESISTANCE, ParamValue::Float(0.085));
1151        params.set_by_id(ParamId::PARAM_MOTOR_KV, ParamValue::Float(0.02894));
1152        params.set_by_id(ParamId::PARAM_NO_LOAD_CURRENT, ParamValue::Float(1.01));
1153        params.set_by_id(ParamId::PARAM_PROP_DIAMETER, ParamValue::Float(0.381));
1154        params.set_by_id(ParamId::PARAM_PROP_CQ, ParamValue::Float(0.0045));
1155
1156        for output in 0..4 {
1157            params.set_by_id(
1158                ParamId::from_index(ParamId::PARAM_PRIMARY_MIXER_OUTPUT_0 as usize + output)
1159                    .unwrap(),
1160                ParamValue::Int(2),
1161            );
1162            params.set_by_id(
1163                ParamId::from_index(ParamId::PARAM_PRIMARY_MIXER_PWM_RATE_0 as usize + output)
1164                    .unwrap(),
1165                ParamValue::Float(490.0),
1166            );
1167            params.set_by_id(
1168                ParamId::from_index(
1169                    ParamId::PARAM_PRIMARY_MIXER_0_0 as usize + output * NUM_MIXER_OUTPUTS + 2,
1170                )
1171                .unwrap(),
1172                ParamValue::Float(-5814.7935),
1173            );
1174        }
1175
1176        let state = StateManager::new();
1177        let mut mixer = MatrixMixer::<f64>::new(&params);
1178        let mut controls = ControllerOutput::<f64>::default();
1179        controls.u[2] = -25.0;
1180
1181        let run = mixer.mix(
1182            &controls,
1183            MixerCtx {
1184                battery_voltage: Some(23.5),
1185                ..mixer_ctx(&state, &params, OVERRIDE_NO_OVERRIDE)
1186            },
1187        );
1188
1189        assert_eq!(run.status, MixerStatus::Healthy);
1190        for output in 0..4 {
1191            assert!(
1192                run.commands[output] > 0.4 && run.commands[output] < 0.6,
1193                "motor {output} output was {}",
1194                run.commands[output]
1195            );
1196        }
1197    }
1198}
veloxity_core/mixer/matrix.rs

veloxity_core/mixer/
matrix.rs
