veloxity_core/controller/
quad.rs

1use super::{Controller, RcTrimCalibrator};
2use crate::command::{CombinedControl, ControlType};
3use crate::controller::ControllerCtx;
4use crate::estimator::quad::AttitudeState;
5use crate::math::{FlightFloat, pi};
6use crate::params::{ParamId, ParamValue, Params};
7use nalgebra::Quaternion;
8use nalgebra::SVector as Vector;
9
10/// Clamps a value between a lower and upper bound. Needed because controller is generic over R, so can run f32 or f64 and won't automatically treat 0.01 as generic type R so we have to help it
11fn r<R: FlightFloat>(value: f32) -> R {
12    <R as FlightFloat>::from_f32(value)
13}
14
15fn clamp<R: FlightFloat>(value: R, min: R, max: R) -> R {
16    if value < min {
17        min
18    } else if value > max {
19        max
20    } else {
21        value
22    }
23}
24
25#[derive(Debug, Clone, Copy)]
26pub struct Pid<R: FlightFloat> {
27    pub p: R,
28    pub i: R,
29    pub d: R,
30    pub max: R,
31    pub min: R,
32    pub tau: R,
33    pub integrator: R,
34    pub differentiator: R,
35    pub prev_x: R,
36    pub prev_t: R,
37}
38
39impl<R: FlightFloat> Default for Pid<R> {
40    fn default() -> Self {
41        Self {
42            p: r::<R>(0.0),
43            i: r::<R>(0.0),
44            d: r::<R>(0.0),
45            max: R::infinity(),
46            min: -R::infinity(),
47            tau: r::<R>(0.05),
48            integrator: r::<R>(0.0),
49            differentiator: r::<R>(0.0),
50            prev_x: r::<R>(0.0),
51            prev_t: r::<R>(-1.0),
52        }
53    }
54}
55
56impl<R: FlightFloat> Pid<R> {
57    pub fn new(p: R, i: R, d: R, max_i: R, tau: R) -> Self {
58        Self {
59            p,
60            i,
61            d,
62            max: max_i,
63            min: -max_i,
64            tau,
65            integrator: r::<R>(0.0),
66            differentiator: r::<R>(0.0),
67            prev_x: r::<R>(0.0),
68            prev_t: r::<R>(-1.0),
69        }
70    }
71    pub fn run(&mut self, x: R, x_c: R, dt: R, enable_integrator: bool) -> R {
72        let xdot = if dt > r::<R>(0.0001) {
73            self.differentiator = (r::<R>(2.0) * self.tau - dt) / (r::<R>(2.0) * self.tau + dt)
74                * self.differentiator
75                + r::<R>(2.0) / (r::<R>(2.0) * self.tau + dt) * (x - self.prev_x);
76            self.differentiator
77        } else {
78            r::<R>(0.0)
79        };
80        self.prev_x = x;
81
82        self.run_with_derivative(x, x_c, xdot, dt, enable_integrator)
83    }
84
85    pub fn run_with_derivative(
86        &mut self,
87        x: R,
88        x_c: R,
89        xdot: R,
90        dt: R,
91        enable_integrator: bool,
92    ) -> R {
93        let error = x_c - x;
94
95        let p_term = self.p * error;
96        let d_term = if self.d > r::<R>(0.0) {
97            self.d * xdot
98        } else {
99            r::<R>(0.0)
100        };
101
102        let mut i_term = r::<R>(0.0);
103        if self.i > r::<R>(0.0) && enable_integrator {
104            self.integrator += error * dt;
105            i_term = self.i * self.integrator;
106        }
107
108        let output = p_term - d_term + i_term;
109        let saturated = clamp(output, self.min, self.max);
110
111        if output != saturated
112            && self.i > r::<R>(0.0)
113            && i_term.abs() > (output - p_term + d_term).abs()
114        {
115            self.integrator = (saturated - p_term + d_term) / self.i;
116        }
117
118        saturated
119    }
120
121    pub fn reset(&mut self) {
122        self.integrator = r::<R>(0.0);
123        self.differentiator = r::<R>(0.0);
124        self.prev_x = r::<R>(0.0);
125        self.prev_t = r::<R>(-1.0);
126    }
127}
128
129#[derive(Debug, Clone, Copy, PartialEq)]
130pub struct ControllerOutput<R: FlightFloat> {
131    pub u: [R; 10],
132}
133
134impl<R: FlightFloat> ControllerOutput<R> {
135    pub fn from_forces_torques_and_passthrough(
136        forces: Vector<R, 3>,
137        torques: Vector<R, 3>,
138        passthrough: [R; 4],
139    ) -> Self {
140        Self {
141            u: [
142                forces[0],
143                forces[1],
144                forces[2],
145                torques[0],
146                torques[1],
147                torques[2],
148                passthrough[0],
149                passthrough[1],
150                passthrough[2],
151                passthrough[3],
152            ],
153        }
154    }
155
156    pub fn forces(&self) -> Vector<R, 3> {
157        Vector::from([self.u[0], self.u[1], self.u[2]])
158    }
159
160    pub fn torques(&self) -> Vector<R, 3> {
161        Vector::from([self.u[3], self.u[4], self.u[5]])
162    }
163
164    pub fn passthrough(&self) -> [R; 4] {
165        [self.u[6], self.u[7], self.u[8], self.u[9]]
166    }
167
168    pub fn quad_thrust_command(&self) -> R {
169        -self.u[2]
170    }
171}
172
173impl<R: FlightFloat> Default for ControllerOutput<R> {
174    fn default() -> Self {
175        Self {
176            u: [r::<R>(0.0); 10],
177        }
178    }
179}
180
181#[derive(Debug, Clone, Copy)]
182struct MotorThrustModel<R: FlightFloat> {
183    resistance: R,
184    diameter_4: R,
185    diameter_5: R,
186    cq: R,
187    ct: R,
188    kv: R,
189    no_load_current: R,
190    num_motors: R,
191    max_voltage: R,
192    pi_2: R,
193}
194
195impl<R: FlightFloat> Default for MotorThrustModel<R> {
196    fn default() -> Self {
197        Self {
198            resistance: r::<R>(0.0),
199            diameter_4: r::<R>(0.0),
200            diameter_5: r::<R>(0.0),
201            cq: r::<R>(0.0),
202            ct: r::<R>(0.0),
203            kv: r::<R>(0.0),
204            no_load_current: r::<R>(0.0),
205            num_motors: r::<R>(0.0),
206            max_voltage: r::<R>(0.0),
207            pi_2: r::<R>(0.0),
208        }
209    }
210}
211
212impl<R: FlightFloat> MotorThrustModel<R> {
213    fn from_params(params: &Params) -> Self {
214        let diameter =
215            <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_PROP_DIAMETER));
216        let diameter_2 = diameter * diameter;
217        let pi = pi::<R>();
218        Self {
219            resistance: <R as FlightFloat>::from_f32(param_float(
220                params,
221                ParamId::PARAM_MOTOR_RESISTANCE,
222            )),
223            diameter_4: diameter_2 * diameter_2,
224            diameter_5: diameter_2 * diameter_2 * diameter,
225            cq: <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_PROP_CQ)),
226            ct: <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_PROP_CT)),
227            kv: <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_MOTOR_KV)),
228            no_load_current: <R as FlightFloat>::from_f32(param_float(
229                params,
230                ParamId::PARAM_NO_LOAD_CURRENT,
231            )),
232            num_motors: <R as FlightFloat>::from_i32(param_int(params, ParamId::PARAM_NUM_MOTORS)),
233            max_voltage: <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_VOLT_MAX)),
234            pi_2: pi * pi,
235        }
236    }
237
238    fn calculate_max_thrust(&self, air_density: R) -> R {
239        let a = self.resistance * air_density * self.diameter_5 * self.cq
240            / (r::<R>(4.0) * self.pi_2 * self.kv);
241        let b = self.kv;
242        let c = self.no_load_current * self.resistance - self.max_voltage;
243        let omega = (-b + (b * b - r::<R>(4.0) * a * c).sqrt()) / (r::<R>(2.0) * a);
244
245        air_density * self.diameter_4 * self.ct * omega * omega / (r::<R>(4.0) * self.pi_2)
246            * self.num_motors
247    }
248}
249
250#[derive(Debug, Clone, Copy)]
251pub struct QuadController<R: FlightFloat> {
252    pub roll_rate_pid: Pid<R>,
253    pub pitch_rate_pid: Pid<R>,
254    pub yaw_rate_pid: Pid<R>,
255    pub roll_angle_pid: Pid<R>,
256    pub pitch_angle_pid: Pid<R>,
257    equilibrium_torques: [R; 3],
258    rc_max_throttle: R,
259    use_motor_parameters: bool,
260    motor_thrust_model: MotorThrustModel<R>,
261}
262
263impl<R: FlightFloat> Default for QuadController<R> {
264    fn default() -> Self {
265        Self {
266            roll_rate_pid: Pid::default(),
267            pitch_rate_pid: Pid::default(),
268            yaw_rate_pid: Pid::default(),
269            roll_angle_pid: Pid::default(),
270            pitch_angle_pid: Pid::default(),
271            equilibrium_torques: [r::<R>(0.0); 3],
272            rc_max_throttle: r::<R>(1.0),
273            use_motor_parameters: false,
274            motor_thrust_model: MotorThrustModel::default(),
275        }
276    }
277}
278
279impl<R: FlightFloat> QuadController<R> {
280    pub fn new(
281        roll_rate_pid: Pid<R>,
282        pitch_rate_pid: Pid<R>,
283        yaw_rate_pid: Pid<R>,
284        roll_angle_pid: Pid<R>,
285        pitch_angle_pid: Pid<R>,
286    ) -> Self {
287        Self {
288            roll_rate_pid,
289            pitch_rate_pid,
290            yaw_rate_pid,
291            roll_angle_pid,
292            pitch_angle_pid,
293            equilibrium_torques: [r::<R>(0.0); 3],
294            rc_max_throttle: r::<R>(1.0),
295            use_motor_parameters: false,
296            motor_thrust_model: MotorThrustModel::default(),
297        }
298    }
299
300    fn reset_pids(&mut self) {
301        self.roll_rate_pid.reset();
302        self.pitch_rate_pid.reset();
303        self.yaw_rate_pid.reset();
304        self.roll_angle_pid.reset();
305        self.pitch_angle_pid.reset();
306    }
307
308    fn run_pid_control(
309        &mut self,
310        state: &AttitudeState<R>,
311        command: &CombinedControl,
312        dt: R,
313        add_equilibrium_torques: bool,
314        update_integrators: bool,
315        air_density: R,
316    ) -> ControllerOutput<R> {
317        let current_rates = state.body_rate;
318        let needs_euler = command.qx.control_type == ControlType::Angle
319            || command.qy.control_type == ControlType::Angle;
320        let euler = if needs_euler {
321            Some(Vector::<R, 3>::from(state))
322        } else {
323            None
324        };
325
326        let mut torque_x = match command.qx.control_type {
327            ControlType::Rate => self.roll_rate_pid.run(
328                current_rates[0],
329                <R as FlightFloat>::from_f32(command.qx.value),
330                dt,
331                update_integrators,
332            ),
333            ControlType::Angle => self.roll_angle_pid.run_with_derivative(
334                euler.unwrap()[0],
335                <R as FlightFloat>::from_f32(command.qx.value),
336                current_rates[0],
337                dt,
338                update_integrators,
339            ),
340            _ => <R as FlightFloat>::from_f32(command.qx.value),
341        };
342
343        let mut torque_y = match command.qy.control_type {
344            ControlType::Rate => self.pitch_rate_pid.run(
345                current_rates[1],
346                <R as FlightFloat>::from_f32(command.qy.value),
347                dt,
348                update_integrators,
349            ),
350            ControlType::Angle => self.pitch_angle_pid.run_with_derivative(
351                euler.unwrap()[1],
352                <R as FlightFloat>::from_f32(command.qy.value),
353                current_rates[1],
354                dt,
355                update_integrators,
356            ),
357            _ => <R as FlightFloat>::from_f32(command.qy.value),
358        };
359
360        let mut torque_z = match command.qz.control_type {
361            ControlType::Rate => self.yaw_rate_pid.run(
362                current_rates[2],
363                <R as FlightFloat>::from_f32(command.qz.value),
364                dt,
365                update_integrators,
366            ),
367            _ => <R as FlightFloat>::from_f32(command.qz.value),
368        };
369
370        if add_equilibrium_torques {
371            torque_x += self.equilibrium_torques[0];
372            torque_y += self.equilibrium_torques[1];
373            torque_z += self.equilibrium_torques[2];
374        }
375
376        let max_thrust = if self.use_motor_parameters {
377            self.motor_thrust_model.calculate_max_thrust(air_density)
378        } else {
379            r::<R>(0.0)
380        };
381
382        let forces = Vector::from([
383            force_output(
384                <R as FlightFloat>::from_f32(command.fx.value),
385                command.fx.control_type,
386                false,
387                self.rc_max_throttle,
388                self.use_motor_parameters,
389                max_thrust,
390            ),
391            force_output(
392                <R as FlightFloat>::from_f32(command.fy.value),
393                command.fy.control_type,
394                false,
395                self.rc_max_throttle,
396                self.use_motor_parameters,
397                max_thrust,
398            ),
399            force_output(
400                <R as FlightFloat>::from_f32(command.fz.value),
401                command.fz.control_type,
402                true,
403                self.rc_max_throttle,
404                self.use_motor_parameters,
405                max_thrust,
406            ),
407        ]);
408
409        ControllerOutput::from_forces_torques_and_passthrough(
410            forces,
411            Vector::from([torque_x, torque_y, torque_z]),
412            [
413                <R as FlightFloat>::from_f32(command.passthrough[0].value),
414                <R as FlightFloat>::from_f32(command.passthrough[1].value),
415                <R as FlightFloat>::from_f32(command.passthrough[2].value),
416                <R as FlightFloat>::from_f32(command.passthrough[3].value),
417            ],
418        )
419    }
420}
421
422impl<R: FlightFloat> Controller<R> for QuadController<R> {
423    type State = AttitudeState<R>;
424    type ControlOutput = ControllerOutput<R>;
425
426    fn update_gains(&mut self, params: &Params) {
427        // Roll Rate
428        self.roll_rate_pid.p = match params.get_by_id(ParamId::PARAM_PID_ROLL_RATE_P) {
429            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
430            _ => r::<R>(0.0),
431        };
432        self.roll_rate_pid.i = match params.get_by_id(ParamId::PARAM_PID_ROLL_RATE_I) {
433            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
434            _ => r::<R>(0.0),
435        };
436        self.roll_rate_pid.d = match params.get_by_id(ParamId::PARAM_PID_ROLL_RATE_D) {
437            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
438            _ => r::<R>(0.0),
439        };
440        self.roll_rate_pid.tau = match params.get_by_id(ParamId::PARAM_PID_TAU) {
441            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
442            _ => r::<R>(0.0),
443        };
444
445        // Pitch Rate
446        self.pitch_rate_pid.p = match params.get_by_id(ParamId::PARAM_PID_PITCH_RATE_P) {
447            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
448            _ => r::<R>(0.0),
449        };
450        self.pitch_rate_pid.i = match params.get_by_id(ParamId::PARAM_PID_PITCH_RATE_I) {
451            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
452            _ => r::<R>(0.0),
453        };
454        self.pitch_rate_pid.d = match params.get_by_id(ParamId::PARAM_PID_PITCH_RATE_D) {
455            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
456            _ => r::<R>(0.0),
457        };
458        self.pitch_rate_pid.tau = match params.get_by_id(ParamId::PARAM_PID_TAU) {
459            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
460            _ => r::<R>(0.0),
461        };
462
463        // Yaw Rate
464        self.yaw_rate_pid.p = match params.get_by_id(ParamId::PARAM_PID_YAW_RATE_P) {
465            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
466            _ => r::<R>(0.0),
467        };
468        self.yaw_rate_pid.i = match params.get_by_id(ParamId::PARAM_PID_YAW_RATE_I) {
469            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
470            _ => r::<R>(0.0),
471        };
472        self.yaw_rate_pid.d = match params.get_by_id(ParamId::PARAM_PID_YAW_RATE_D) {
473            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
474            _ => r::<R>(0.0),
475        };
476        self.yaw_rate_pid.tau = match params.get_by_id(ParamId::PARAM_PID_TAU) {
477            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
478            _ => r::<R>(0.0),
479        };
480
481        // Roll Angle
482        self.roll_angle_pid.p = match params.get_by_id(ParamId::PARAM_PID_ROLL_ANGLE_P) {
483            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
484            _ => r::<R>(0.0),
485        };
486        self.roll_angle_pid.i = match params.get_by_id(ParamId::PARAM_PID_ROLL_ANGLE_I) {
487            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
488            _ => r::<R>(0.0),
489        };
490        self.roll_angle_pid.d = match params.get_by_id(ParamId::PARAM_PID_ROLL_ANGLE_D) {
491            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
492            _ => r::<R>(0.0),
493        };
494        self.roll_angle_pid.tau = match params.get_by_id(ParamId::PARAM_PID_TAU) {
495            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
496            _ => r::<R>(0.0),
497        };
498
499        // Pitch Angle
500        self.pitch_angle_pid.p = match params.get_by_id(ParamId::PARAM_PID_PITCH_ANGLE_P) {
501            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
502            _ => r::<R>(0.0),
503        };
504        self.pitch_angle_pid.i = match params.get_by_id(ParamId::PARAM_PID_PITCH_ANGLE_I) {
505            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
506            _ => r::<R>(0.0),
507        };
508        self.pitch_angle_pid.d = match params.get_by_id(ParamId::PARAM_PID_PITCH_ANGLE_D) {
509            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
510            _ => r::<R>(0.0),
511        };
512        self.pitch_angle_pid.tau = match params.get_by_id(ParamId::PARAM_PID_TAU) {
513            ParamValue::Float(val) => <R as FlightFloat>::from_f32(val),
514            _ => r::<R>(0.0),
515        };
516        self.equilibrium_torques = [
517            <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_X_EQ_TORQUE)),
518            <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_Y_EQ_TORQUE)),
519            <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_Z_EQ_TORQUE)),
520        ];
521        self.rc_max_throttle =
522            <R as FlightFloat>::from_f32(param_float(params, ParamId::PARAM_RC_MAX_THROTTLE));
523        self.use_motor_parameters = param_int(params, ParamId::PARAM_USE_MOTOR_PARAMETERS) != 0;
524        self.motor_thrust_model = MotorThrustModel::from_params(params);
525    }
526
527    fn control(&mut self, state: &Self::State, ctx: ControllerCtx<'_, R>) -> Self::ControlOutput {
528        let update_integrators = ctx.state_manager.is_armed()
529            && controller_should_update_integrators(ctx.command, ctx.dt);
530        self.run_pid_control(
531            state,
532            ctx.command,
533            ctx.dt,
534            true,
535            update_integrators,
536            ctx.air_density,
537        )
538    }
539}
540
541impl<R: FlightFloat> RcTrimCalibrator for QuadController<R> {
542    fn calculate_equilibrium_torques_from_rc(
543        &mut self,
544        rc_control: &CombinedControl,
545        params: &Params,
546    ) -> [f32; 3] {
547        let mut controller = *self;
548        controller.update_gains(params);
549        controller.reset_pids();
550        let output = controller.run_pid_control(
551            &AttitudeState::<R>::default(),
552            rc_control,
553            r::<R>(0.0),
554            false,
555            false,
556            r::<R>(1.225),
557        );
558
559        [
560            output.u[3].to_f32_lossy(),
561            output.u[4].to_f32_lossy(),
562            output.u[5].to_f32_lossy(),
563        ]
564    }
565}
566
567fn param_float(params: &Params, id: ParamId) -> f32 {
568    match params.get_by_id(id) {
569        ParamValue::Float(value) => value,
570        _ => 0.0,
571    }
572}
573
574fn param_int(params: &Params, id: ParamId) -> i32 {
575    match params.get_by_id(id) {
576        ParamValue::Int(value) => value,
577        _ => 0,
578    }
579}
580
581fn force_output<R: FlightFloat>(
582    value: R,
583    control_type: ControlType,
584    is_fz: bool,
585    rc_max_throttle: R,
586    use_motor_parameters: bool,
587    max_thrust: R,
588) -> R {
589    if control_type != ControlType::Throttle {
590        return value;
591    }
592
593    let sign = if is_fz { r::<R>(-1.0) } else { r::<R>(1.0) };
594    let mut output = sign * value * rc_max_throttle;
595
596    if use_motor_parameters {
597        output *= max_thrust;
598    }
599
600    output
601}
602
603fn controller_should_update_integrators<R: FlightFloat>(command: &CombinedControl, dt: R) -> bool {
604    dt < r::<R>(0.01)
605        && (command.fx.value > 0.1 || command.fy.value > 0.1 || command.fz.value > 0.1)
606}
607
608/// Constructs a Quaternion from Euler angles (Roll, Pitch, Yaw) ZYX sequence
609/// Roll and Pitch are in radians.
610pub fn quaternion_from_euler<R: FlightFloat>(roll: R, pitch: R, yaw: R) -> Quaternion<R> {
611    // libm 0.2 does not have sin_cos, so we compute them separately
612    let sr = (roll * r::<R>(0.5)).sin();
613    let cr = (roll * r::<R>(0.5)).cos();
614
615    let sp = (pitch * r::<R>(0.5)).sin();
616    let cp = (pitch * r::<R>(0.5)).cos();
617
618    let sy = (yaw * r::<R>(0.5)).sin();
619    let cy = (yaw * r::<R>(0.5)).cos();
620
621    Quaternion::new(
622        cr * cp * cy + sr * sp * sy, // w
623        sr * cp * cy - cr * sp * sy, // x
624        cr * sp * cy + sr * cp * sy, // y
625        cr * cp * sy - sr * sp * cy, // z
626    )
627}
628
629/// Extracts Yaw (Z-axis rotation) from a Quaternion
630pub fn get_yaw<R: FlightFloat>(q: Quaternion<R>) -> R {
631    let w = q.w;
632    let x = q.i;
633    let y = q.j;
634    let z = q.k;
635
636    (r::<R>(2.0) * (w * z + x * y)).atan2(r::<R>(1.0) - r::<R>(2.0) * (y * y + z * z))
637}
638
639#[cfg(test)]
640mod tests {
641    use super::*;
642    use crate::{
643        command::{CombinedControl, ControlChannel, ControlType},
644        state_machine::{Event, StateManager},
645    };
646
647    fn armed_state(params: &Params) -> StateManager {
648        let mut state_manager = StateManager::new();
649        state_manager.update(Event::INITIALIZED, params);
650        state_manager.update_arming_safety(true, true);
651        state_manager.update(Event::REQUEST_ARM, params);
652        state_manager
653    }
654
655    fn control_with_density(
656        controller: &mut QuadController<f64>,
657        state: &AttitudeState<f64>,
658        state_manager: &mut StateManager,
659        command: &CombinedControl,
660        params: &Params,
661        dt: f64,
662        air_density: f64,
663    ) -> ControllerOutput<f64> {
664        controller.update_gains(params);
665        controller.control(
666            state,
667            ControllerCtx {
668                state_manager,
669                command,
670                params,
671                air_density,
672                dt,
673            },
674        )
675    }
676
677    #[test]
678    fn controller_adds_equilibrium_torque_params_to_control_output() {
679        let mut params = Params::new();
680        params.set_by_id(ParamId::PARAM_X_EQ_TORQUE, ParamValue::Float(0.1));
681        params.set_by_id(ParamId::PARAM_Y_EQ_TORQUE, ParamValue::Float(-0.2));
682        params.set_by_id(ParamId::PARAM_Z_EQ_TORQUE, ParamValue::Float(0.3));
683        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
684
685        let mut state_manager = armed_state(&params);
686
687        let mut controller = QuadController::<f64>::default();
688        let state = AttitudeState::<f64>::default();
689        let command = CombinedControl {
690            qx: ControlChannel {
691                active: true,
692                control_type: ControlType::Rate,
693                value: 0.0,
694            },
695            qy: ControlChannel {
696                active: true,
697                control_type: ControlType::Rate,
698                value: 0.0,
699            },
700            qz: ControlChannel {
701                active: true,
702                control_type: ControlType::Rate,
703                value: 0.0,
704            },
705            fz: ControlChannel {
706                active: true,
707                control_type: ControlType::Throttle,
708                value: 0.4,
709            },
710            ..Default::default()
711        };
712
713        let output = control_with_density(
714            &mut controller,
715            &state,
716            &mut state_manager,
717            &command,
718            &params,
719            0.0025,
720            1.225,
721        );
722
723        assert_eq!(output.u[3], 0.10000000149011612);
724        assert_eq!(output.u[4], -0.20000000298023224);
725        assert_eq!(output.u[5], 0.30000001192092896);
726        assert!((output.u[2] + 0.28).abs() < 1e-6);
727    }
728
729    #[test]
730    fn rc_trim_calibration_uses_pid_output_without_existing_equilibrium_torques() {
731        let mut params = Params::new();
732        params.set_by_id(ParamId::PARAM_PID_ROLL_RATE_P, ParamValue::Float(2.0));
733        params.set_by_id(ParamId::PARAM_PID_PITCH_RATE_P, ParamValue::Float(3.0));
734        params.set_by_id(ParamId::PARAM_PID_YAW_RATE_P, ParamValue::Float(4.0));
735        params.set_by_id(ParamId::PARAM_X_EQ_TORQUE, ParamValue::Float(0.5));
736        params.set_by_id(ParamId::PARAM_Y_EQ_TORQUE, ParamValue::Float(-0.5));
737        params.set_by_id(ParamId::PARAM_Z_EQ_TORQUE, ParamValue::Float(0.25));
738
739        let command = CombinedControl {
740            qx: ControlChannel {
741                active: true,
742                control_type: ControlType::Rate,
743                value: 0.1,
744            },
745            qy: ControlChannel {
746                active: true,
747                control_type: ControlType::Rate,
748                value: -0.1,
749            },
750            qz: ControlChannel {
751                active: true,
752                control_type: ControlType::Rate,
753                value: 0.2,
754            },
755            fz: ControlChannel {
756                active: true,
757                control_type: ControlType::Throttle,
758                value: 0.4,
759            },
760            ..Default::default()
761        };
762
763        let mut controller = QuadController::<f64>::default();
764        let torques = controller.calculate_equilibrium_torques_from_rc(&command, &params);
765
766        assert_eq!(torques[0], 0.2);
767        assert_eq!(torques[1], -0.3);
768        assert_eq!(torques[2], 0.8);
769    }
770
771    #[test]
772    fn controller_output_preserves_rosflight_ten_channel_shape() {
773        let mut params = Params::new();
774        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
775
776        let mut state_manager = armed_state(&params);
777        let mut controller = QuadController::<f64>::default();
778        let state = AttitudeState::<f64>::default();
779        let command = CombinedControl {
780            qx: ControlChannel {
781                active: true,
782                control_type: ControlType::Passthrough,
783                value: 0.1,
784            },
785            qy: ControlChannel {
786                active: true,
787                control_type: ControlType::Passthrough,
788                value: 0.2,
789            },
790            qz: ControlChannel {
791                active: true,
792                control_type: ControlType::Passthrough,
793                value: 0.3,
794            },
795            fx: ControlChannel {
796                active: true,
797                control_type: ControlType::Passthrough,
798                value: 0.4,
799            },
800            fy: ControlChannel {
801                active: true,
802                control_type: ControlType::Passthrough,
803                value: 0.5,
804            },
805            fz: ControlChannel {
806                active: true,
807                control_type: ControlType::Passthrough,
808                value: 0.6,
809            },
810            passthrough: [
811                ControlChannel {
812                    active: true,
813                    control_type: ControlType::Passthrough,
814                    value: 0.7,
815                },
816                ControlChannel {
817                    active: true,
818                    control_type: ControlType::Passthrough,
819                    value: 0.8,
820                },
821                ControlChannel {
822                    active: true,
823                    control_type: ControlType::Passthrough,
824                    value: 0.9,
825                },
826                ControlChannel {
827                    active: true,
828                    control_type: ControlType::Passthrough,
829                    value: 1.0,
830                },
831            ],
832            stamp_ms: 0,
833        };
834
835        let output = control_with_density(
836            &mut controller,
837            &state,
838            &mut state_manager,
839            &command,
840            &params,
841            0.0025,
842            1.225,
843        );
844
845        for (actual, expected) in output
846            .u
847            .iter()
848            .zip([0.4, 0.5, 0.6, 0.1, 0.2, 0.3, 0.7, 0.8, 0.9, 1.0])
849        {
850            assert!((*actual - expected).abs() < 1e-6);
851        }
852    }
853
854    #[test]
855    fn pid_integrator_updates_only_when_rosflight_gate_allows_it() {
856        let mut params = Params::new();
857        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
858        params.set_by_id(ParamId::PARAM_PID_ROLL_RATE_P, ParamValue::Float(0.0));
859        params.set_by_id(ParamId::PARAM_PID_ROLL_RATE_I, ParamValue::Float(2.0));
860        params.set_by_id(ParamId::PARAM_PID_ROLL_RATE_D, ParamValue::Float(0.0));
861
862        let state = AttitudeState::<f64>::default();
863        let command = CombinedControl {
864            qx: ControlChannel {
865                active: true,
866                control_type: ControlType::Rate,
867                value: 1.0,
868            },
869            fz: ControlChannel {
870                active: true,
871                control_type: ControlType::Throttle,
872                value: 0.2,
873            },
874            ..Default::default()
875        };
876
877        let mut gated_out_state = armed_state(&params);
878        let mut gated_out_controller = QuadController::<f64>::default();
879        let gated_out = control_with_density(
880            &mut gated_out_controller,
881            &state,
882            &mut gated_out_state,
883            &command,
884            &params,
885            0.02,
886            1.225,
887        );
888        assert_eq!(gated_out.u[3], 0.0);
889
890        let mut gated_in_state = armed_state(&params);
891        let mut gated_in_controller = QuadController::<f64>::default();
892        let gated_in = control_with_density(
893            &mut gated_in_controller,
894            &state,
895            &mut gated_in_state,
896            &command,
897            &params,
898            0.005,
899            1.225,
900        );
901
902        assert!((gated_in.u[3] - 0.01).abs() < 1e-9);
903    }
904
905    #[test]
906    fn pid_derivative_integrator_and_saturation_match_rosflight_trace() {
907        let mut pid = Pid::<f64>::new(2.0, 3.0, 0.5, 0.25, 0.05);
908
909        let first = pid.run(1.0, 3.0, 0.01, true);
910        assert!((pid.differentiator - 18.1818181818).abs() < 1e-9);
911        assert!((pid.integrator - 0.02).abs() < 1e-9);
912        assert_eq!(first, -0.25);
913
914        let second = pid.run(1.2, 3.0, 0.01, true);
915        assert!((pid.differentiator - 18.5123966942).abs() < 1e-9);
916        assert!((pid.integrator - 1.8020661157).abs() < 1e-9);
917        assert_eq!(second, -0.25);
918
919        let held_integrator = pid.integrator;
920        let disabled = pid.run(1.2, 3.0, 0.01, false);
921        assert_eq!(pid.integrator, held_integrator);
922        assert_eq!(disabled, -0.25);
923    }
924
925    #[test]
926    fn angle_mode_controller_trace_uses_body_rate_as_derivative_feedback() {
927        let mut params = Params::new();
928        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
929        params.set_by_id(ParamId::PARAM_PID_ROLL_ANGLE_P, ParamValue::Float(4.0));
930        params.set_by_id(ParamId::PARAM_PID_ROLL_ANGLE_I, ParamValue::Float(0.0));
931        params.set_by_id(ParamId::PARAM_PID_ROLL_ANGLE_D, ParamValue::Float(0.5));
932        params.set_by_id(ParamId::PARAM_PID_PITCH_ANGLE_P, ParamValue::Float(3.0));
933        params.set_by_id(ParamId::PARAM_PID_PITCH_ANGLE_I, ParamValue::Float(0.0));
934        params.set_by_id(ParamId::PARAM_PID_PITCH_ANGLE_D, ParamValue::Float(0.25));
935        params.set_by_id(ParamId::PARAM_PID_YAW_RATE_P, ParamValue::Float(2.0));
936        params.set_by_id(ParamId::PARAM_PID_YAW_RATE_I, ParamValue::Float(0.0));
937        params.set_by_id(ParamId::PARAM_PID_YAW_RATE_D, ParamValue::Float(0.0));
938
939        let mut state_manager = armed_state(&params);
940        let mut controller = QuadController::<f64>::default();
941        let state = AttitudeState {
942            q_hat: quaternion_from_euler::<f64>(0.1, -0.2, 0.0),
943            body_rate: Vector::from([0.3, -0.4, 0.5]),
944            is_healthy: true,
945            ..Default::default()
946        };
947        let command = CombinedControl {
948            qx: ControlChannel {
949                active: true,
950                control_type: ControlType::Angle,
951                value: 0.2,
952            },
953            qy: ControlChannel {
954                active: true,
955                control_type: ControlType::Angle,
956                value: -0.1,
957            },
958            qz: ControlChannel {
959                active: true,
960                control_type: ControlType::Rate,
961                value: 0.8,
962            },
963            fz: ControlChannel {
964                active: true,
965                control_type: ControlType::Throttle,
966                value: 0.4,
967            },
968            ..Default::default()
969        };
970
971        let output = control_with_density(
972            &mut controller,
973            &state,
974            &mut state_manager,
975            &command,
976            &params,
977            0.005,
978            1.225,
979        );
980
981        assert!((output.u[3] - 0.25).abs() < 1e-6);
982        assert!((output.u[4] - 0.4).abs() < 1e-6);
983        assert!((output.u[5] - 0.6).abs() < 1e-6);
984        assert!((output.u[2] + 0.28).abs() < 1e-6);
985    }
986
987    #[test]
988    fn motor_param_thrust_scaling_uses_controller_air_density_context() {
989        let mut params = Params::new();
990        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
991        params.set_by_id(ParamId::PARAM_USE_MOTOR_PARAMETERS, ParamValue::Int(1));
992
993        let state = AttitudeState::<f64>::default();
994        let command = CombinedControl {
995            fz: ControlChannel {
996                active: true,
997                control_type: ControlType::Throttle,
998                value: 0.4,
999            },
1000            ..Default::default()
1001        };
1002
1003        let mut lower_density_state = armed_state(&params);
1004        let mut lower_density_controller = QuadController::<f64>::default();
1005        let lower_density_output = control_with_density(
1006            &mut lower_density_controller,
1007            &state,
1008            &mut lower_density_state,
1009            &command,
1010            &params,
1011            0.005,
1012            1.0,
1013        );
1014
1015        let mut higher_density_state = armed_state(&params);
1016        let mut higher_density_controller = QuadController::<f64>::default();
1017        let higher_density_output = control_with_density(
1018            &mut higher_density_controller,
1019            &state,
1020            &mut higher_density_state,
1021            &command,
1022            &params,
1023            0.005,
1024            1.3,
1025        );
1026
1027        assert!(higher_density_output.u[2] < lower_density_output.u[2]);
1028    }
1029}
veloxity_core/controller/quad.rs

veloxity_core/controller/
quad.rs
