veloxity_core/pwm/
output_sync.rs

1use crate::{
2    board::BoardIo,
3    comm::messages::{enums::RosflightAuxCmdType, messages::RosflightAuxCmdMsg},
4    math::FlightFloat,
5    mixer::MixerOutputType,
6    params::{ParamId, ParamValue, Params},
7    pwm::{PwmDriver, PwmError, safe_disarmed_command},
8    state_machine::StateManager,
9};
10
11pub const PWM_OUTPUT_CHANNELS: usize = 14;
12
13#[derive(Debug, Clone, Copy, PartialEq, Eq)]
14pub struct PwmOutputState {
15    enabled_mask: u32,
16}
17
18impl PwmOutputState {
19    pub fn new(enabled: bool) -> Self {
20        Self {
21            enabled_mask: if enabled { u32::MAX } else { 0 },
22        }
23    }
24
25    pub fn is_enabled(&self) -> bool {
26        self.enabled_mask != 0
27    }
28
29    pub fn enabled_mask(&self) -> u32 {
30        self.enabled_mask
31    }
32}
33
34pub struct PwmSyncCtx<'a, B, P>
35where
36    B: BoardIo,
37{
38    pub board: &'a mut B,
39    pub pwm: &'a mut P,
40    pub output: &'a mut PwmOutputState,
41    pub output_kill_active: bool,
42    pub channel_output_mask: i32,
43}
44
45pub fn sync_pwm_output_state<B, P, R>(ctx: PwmSyncCtx<'_, B, P>) -> Result<bool, PwmError>
46where
47    B: BoardIo,
48    P: PwmDriver<R>,
49    R: FlightFloat,
50{
51    let channel_count = ctx.pwm.len().min(u32::BITS as usize);
52    let available_mask = if channel_count == u32::BITS as usize {
53        u32::MAX
54    } else {
55        (1_u32 << channel_count) - 1
56    };
57    let desired_mask = if ctx.output_kill_active {
58        0
59    } else if ctx.channel_output_mask == -1 {
60        available_mask
61    } else if ctx.channel_output_mask >= 0 {
62        ctx.channel_output_mask as u32 & available_mask
63    } else {
64        0
65    };
66    let current_mask = ctx.output.enabled_mask & available_mask;
67
68    if desired_mask == current_mask {
69        ctx.output.enabled_mask = current_mask;
70        return Ok(false);
71    }
72
73    if desired_mask == available_mask && current_mask == 0 {
74        ctx.pwm.enable_all()?;
75        ctx.output.enabled_mask = desired_mask;
76        return Ok(true);
77    }
78
79    if desired_mask == 0 {
80        ctx.pwm.disable_all();
81        ctx.pwm.flush(ctx.board);
82        ctx.output.enabled_mask = 0;
83        return Ok(true);
84    }
85
86    let mut applied_mask = current_mask;
87    let disable_mask = current_mask & !desired_mask;
88    for channel in 0..channel_count {
89        let bit = 1_u32 << channel;
90        if disable_mask & bit != 0 {
91            ctx.pwm.disable(channel)?;
92            applied_mask &= !bit;
93            ctx.output.enabled_mask = applied_mask;
94        }
95    }
96    if disable_mask != 0 {
97        ctx.pwm.flush(ctx.board);
98    }
99
100    let enable_mask = desired_mask & !current_mask;
101    for channel in 0..channel_count {
102        let bit = 1_u32 << channel;
103        if enable_mask & bit != 0 {
104            ctx.pwm.enable(channel)?;
105            applied_mask |= bit;
106            ctx.output.enabled_mask = applied_mask;
107        }
108    }
109
110    Ok(true)
111}
112
113pub fn write_pwm_commands<B, P, R>(
114    board: &mut B,
115    pwm: &mut P,
116    output: &PwmOutputState,
117    commands: &[R],
118    output_types: &[MixerOutputType],
119    state: &StateManager,
120) -> Result<bool, PwmError>
121where
122    B: BoardIo,
123    P: PwmDriver<R>,
124    R: FlightFloat,
125{
126    if !output.is_enabled() {
127        return Ok(false);
128    }
129
130    if state.is_armed() {
131        pwm.send_commands(board, commands)?;
132    } else {
133        pwm.send_disarmed_commands(board, output_types)?;
134    }
135    Ok(true)
136}
137
138pub fn compose_pwm_outputs<R: FlightFloat>(
139    primary_commands: &[R],
140    primary_output_types: &[MixerOutputType],
141    aux_command: Option<&RosflightAuxCmdMsg>,
142    state: &StateManager,
143    params: &Params,
144) -> [R; PWM_OUTPUT_CHANNELS] {
145    let idle_throttle = match params.get_by_id(ParamId::PARAM_MOTOR_IDLE_THROTTLE) {
146        ParamValue::Float(value) => <R as FlightFloat>::from_f32(value),
147        _ => <R as FlightFloat>::from_f32(0.0),
148    };
149    let spin_when_armed = match params.get_by_id(ParamId::PARAM_SPIN_MOTORS_WHEN_ARMED) {
150        ParamValue::Int(value) => value != 0,
151        _ => false,
152    };
153    let channel_output_mask = match params.get_by_id(ParamId::PARAM_CHANNEL_OUTPUT_MASK) {
154        ParamValue::Int(value) => value,
155        _ => 0,
156    };
157
158    let mut outputs = [<R as FlightFloat>::from_f32(0.0); PWM_OUTPUT_CHANNELS];
159
160    for channel in 0..PWM_OUTPUT_CHANNELS {
161        let primary_type = primary_output_types
162            .get(channel)
163            .copied()
164            .unwrap_or(MixerOutputType::Aux);
165        let primary_value = primary_commands
166            .get(channel)
167            .copied()
168            .unwrap_or_else(|| <R as FlightFloat>::from_f32(0.0));
169        let (output_type, value) = if primary_type == MixerOutputType::Aux {
170            aux_output_for_channel(aux_command, channel)
171        } else {
172            (primary_type, primary_value)
173        };
174
175        outputs[channel] = if !state.is_armed() {
176            safe_disarmed_command(output_type)
177        } else if !channel_output_enabled(channel_output_mask, channel) {
178            safe_disarmed_command(output_type)
179        } else {
180            raw_output_for_type(output_type, value, state, idle_throttle, spin_when_armed)
181        };
182    }
183
184    outputs
185}
186
187fn channel_output_enabled(mask: i32, channel: usize) -> bool {
188    mask == -1 || (mask >= 0 && channel < i32::BITS as usize && (mask & (1_i32 << channel)) != 0)
189}
190
191fn aux_output_for_channel<R: FlightFloat>(
192    aux_command: Option<&RosflightAuxCmdMsg>,
193    channel: usize,
194) -> (MixerOutputType, R) {
195    let Some(aux_command) = aux_command else {
196        return (MixerOutputType::Aux, <R as FlightFloat>::from_f32(0.0));
197    };
198
199    match aux_command.type_array[channel] {
200        RosflightAuxCmdType::Disabled => (MixerOutputType::Aux, <R as FlightFloat>::from_f32(0.0)),
201        RosflightAuxCmdType::Servo => (
202            MixerOutputType::Servo,
203            <R as FlightFloat>::from_f32(aux_command.aux_cmd_array[channel]),
204        ),
205        RosflightAuxCmdType::Motor => (
206            MixerOutputType::Motor,
207            <R as FlightFloat>::from_f32(aux_command.aux_cmd_array[channel]),
208        ),
209    }
210}
211
212fn raw_output_for_type<R: FlightFloat>(
213    output_type: MixerOutputType,
214    value: R,
215    state: &StateManager,
216    idle_throttle: R,
217    spin_when_armed: bool,
218) -> R {
219    match output_type {
220        MixerOutputType::Aux => <R as FlightFloat>::from_f32(0.0),
221        MixerOutputType::Servo => {
222            value.clamp(
223                <R as FlightFloat>::from_f32(-1.0),
224                <R as FlightFloat>::from_f32(1.0),
225            ) * <R as FlightFloat>::from_f32(0.5)
226                + <R as FlightFloat>::from_f32(0.5)
227        }
228        MixerOutputType::Gpio => {
229            if value > <R as FlightFloat>::from_f32(0.0) {
230                <R as FlightFloat>::from_f32(1.0)
231            } else {
232                <R as FlightFloat>::from_f32(0.0)
233            }
234        }
235        MixerOutputType::Motor => {
236            if !state.is_armed() {
237                <R as FlightFloat>::from_f32(0.0)
238            } else if value > <R as FlightFloat>::from_f32(1.0) {
239                <R as FlightFloat>::from_f32(1.0)
240            } else if value < idle_throttle && spin_when_armed {
241                idle_throttle
242            } else if value < <R as FlightFloat>::from_f32(0.0) {
243                <R as FlightFloat>::from_f32(0.0)
244            } else {
245                value
246            }
247        }
248    }
249}
250
251#[cfg(test)]
252mod tests {
253    use super::*;
254    use crate::{
255        errors,
256        params::{ParamId, ParamValue, Params},
257        state_machine::Event,
258    };
259
260    struct TestBoard {
261        now_us: u64,
262    }
263
264    impl BoardIo for TestBoard {
265        fn serial_rx_read(&mut self, _buf: &mut [u8]) -> Option<Result<usize, errors::TelemError>> {
266            None
267        }
268
269        fn serial_tx_write(&mut self, bytes: &[u8]) -> Option<Result<usize, errors::TelemError>> {
270            Some(Ok(bytes.len()))
271        }
272
273        fn clock_millis(&self) -> u32 {
274            (self.now_us / 1000) as u32
275        }
276
277        fn clock_micros(&self) -> u64 {
278            self.now_us
279        }
280    }
281
282    struct TestPwm {
283        enabled_mask: u32,
284        enable_all_count: usize,
285        disable_all_count: usize,
286        flush_count: usize,
287        send_count: usize,
288    }
289
290    impl TestPwm {
291        fn new(enabled: bool) -> Self {
292            Self {
293                enabled_mask: if enabled { 0b1111 } else { 0 },
294                enable_all_count: 0,
295                disable_all_count: 0,
296                flush_count: 0,
297                send_count: 0,
298            }
299        }
300    }
301
302    impl PwmDriver<f64> for TestPwm {
303        fn len(&self) -> usize {
304            4
305        }
306
307        fn is_enabled(&self) -> bool {
308            self.enabled_mask == 0b1111
309        }
310
311        fn enable(&mut self, channel: usize) -> Result<(), PwmError> {
312            self.enabled_mask |= 1 << channel;
313            Ok(())
314        }
315
316        fn disable(&mut self, channel: usize) -> Result<(), PwmError> {
317            self.enabled_mask &= !(1 << channel);
318            Ok(())
319        }
320
321        fn enable_all(&mut self) -> Result<(), PwmError> {
322            self.enabled_mask = 0b1111;
323            self.enable_all_count += 1;
324            Ok(())
325        }
326
327        fn disable_all(&mut self) {
328            self.enabled_mask = 0;
329            self.disable_all_count += 1;
330        }
331
332        fn set_duty_cycle(&mut self, _channel: usize, _duty: u16) -> Result<(), PwmError> {
333            Ok(())
334        }
335
336        fn flush<Board: BoardIo>(&mut self, _board: &mut Board) {
337            self.flush_count += 1;
338        }
339
340        fn send_commands<Board: BoardIo>(
341            &mut self,
342            _board: &mut Board,
343            _commands: &[f64],
344        ) -> Result<(), PwmError> {
345            self.send_count += 1;
346            Ok(())
347        }
348    }
349
350    #[test]
351    fn pwm_output_state_defaults_to_enabled_safe_disarmed_outputs() {
352        let mut params = Params::new();
353        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
354        let mut state = StateManager::new();
355        state.update(Event::INITIALIZED, &params);
356        let mut board = TestBoard { now_us: 0 };
357        let mut pwm = TestPwm::new(false);
358        let mut output = PwmOutputState::new(pwm.is_enabled());
359
360        assert!(
361            sync_pwm_output_state(PwmSyncCtx {
362                board: &mut board,
363                pwm: &mut pwm,
364                output: &mut output,
365                output_kill_active: false,
366                channel_output_mask: -1,
367            })
368            .unwrap()
369        );
370        assert!(output.is_enabled());
371        assert_eq!(pwm.enable_all_count, 1);
372        assert_eq!(pwm.disable_all_count, 0);
373
374        state.update_arming_safety(true, true);
375        state.update(Event::REQUEST_ARM, &params);
376
377        assert!(
378            !sync_pwm_output_state(PwmSyncCtx {
379                board: &mut board,
380                pwm: &mut pwm,
381                output: &mut output,
382                output_kill_active: false,
383                channel_output_mask: -1,
384            })
385            .unwrap()
386        );
387        assert!(output.is_enabled());
388        assert_eq!(pwm.enable_all_count, 1);
389
390        assert!(
391            !sync_pwm_output_state(PwmSyncCtx {
392                board: &mut board,
393                pwm: &mut pwm,
394                output: &mut output,
395                output_kill_active: false,
396                channel_output_mask: -1,
397            })
398            .unwrap()
399        );
400        assert_eq!(pwm.enable_all_count, 1);
401
402        state.update(Event::REQUEST_DISARM, &params);
403
404        assert!(
405            !sync_pwm_output_state(PwmSyncCtx {
406                board: &mut board,
407                pwm: &mut pwm,
408                output: &mut output,
409                output_kill_active: false,
410                channel_output_mask: -1,
411            })
412            .unwrap()
413        );
414        assert!(output.is_enabled());
415        assert_eq!(pwm.disable_all_count, 0);
416        assert_eq!(pwm.flush_count, 0);
417    }
418
419    #[test]
420    fn pwm_output_state_physically_applies_channel_mask() {
421        let mut board = TestBoard { now_us: 0 };
422        let mut pwm = TestPwm::new(false);
423        let mut output = PwmOutputState::new(false);
424
425        assert!(
426            sync_pwm_output_state(PwmSyncCtx {
427                board: &mut board,
428                pwm: &mut pwm,
429                output: &mut output,
430                output_kill_active: false,
431                channel_output_mask: 0b0011,
432            })
433            .unwrap()
434        );
435        assert_eq!(pwm.enabled_mask, 0b0011);
436        assert_eq!(output.enabled_mask(), 0b0011);
437        assert_eq!(pwm.enable_all_count, 0);
438
439        assert!(
440            sync_pwm_output_state(PwmSyncCtx {
441                board: &mut board,
442                pwm: &mut pwm,
443                output: &mut output,
444                output_kill_active: false,
445                channel_output_mask: 0b0101,
446            })
447            .unwrap()
448        );
449        assert_eq!(pwm.enabled_mask, 0b0101);
450        assert_eq!(output.enabled_mask(), 0b0101);
451        assert_eq!(pwm.flush_count, 1);
452    }
453
454    #[test]
455    fn pwm_output_state_disables_outputs_while_kill_switch_is_active() {
456        let mut params = Params::new();
457        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
458        let mut state = StateManager::new();
459        state.update(Event::INITIALIZED, &params);
460        state.update_arming_safety(true, true);
461        state.update(Event::REQUEST_ARM, &params);
462        let mut board = TestBoard { now_us: 0 };
463        let mut pwm = TestPwm::new(false);
464        let mut output = PwmOutputState::new(pwm.is_enabled());
465
466        assert!(
467            sync_pwm_output_state(PwmSyncCtx {
468                board: &mut board,
469                pwm: &mut pwm,
470                output: &mut output,
471                output_kill_active: false,
472                channel_output_mask: -1,
473            })
474            .unwrap()
475        );
476        assert!(output.is_enabled());
477
478        assert!(
479            sync_pwm_output_state(PwmSyncCtx {
480                board: &mut board,
481                pwm: &mut pwm,
482                output: &mut output,
483                output_kill_active: true,
484                channel_output_mask: -1,
485            })
486            .unwrap()
487        );
488        assert!(!output.is_enabled());
489        assert_eq!(pwm.disable_all_count, 1);
490        assert_eq!(pwm.flush_count, 1);
491
492        assert!(
493            sync_pwm_output_state(PwmSyncCtx {
494                board: &mut board,
495                pwm: &mut pwm,
496                output: &mut output,
497                output_kill_active: false,
498                channel_output_mask: -1,
499            })
500            .unwrap()
501        );
502        assert!(output.is_enabled());
503        assert_eq!(pwm.enable_all_count, 2);
504    }
505
506    #[test]
507    fn write_pwm_commands_only_writes_when_output_enabled() {
508        let mut board = TestBoard { now_us: 0 };
509        let mut pwm = TestPwm::new(false);
510        let disabled = PwmOutputState::new(false);
511        let enabled = PwmOutputState::new(true);
512        let params = Params::new();
513        let mut state = StateManager::new();
514        state.update(Event::INITIALIZED, &params);
515
516        assert_eq!(
517            write_pwm_commands(&mut board, &mut pwm, &disabled, &[0.1, 0.2], &[], &state),
518            Ok(false)
519        );
520        assert_eq!(pwm.send_count, 0);
521
522        state.update_arming_safety(true, true);
523        state.update(Event::REQUEST_ARM, &params);
524        assert_eq!(
525            write_pwm_commands(&mut board, &mut pwm, &enabled, &[0.1, 0.2], &[], &state),
526            Ok(true)
527        );
528        assert_eq!(pwm.send_count, 1);
529    }
530
531    #[test]
532    fn write_pwm_commands_sends_safe_defaults_when_disarmed() {
533        let mut board = TestBoard { now_us: 0 };
534        let mut pwm = TestPwm::new(false);
535        let enabled = PwmOutputState::new(true);
536        let params = Params::new();
537        let mut state = StateManager::new();
538        state.update(Event::INITIALIZED, &params);
539
540        assert_eq!(
541            write_pwm_commands(
542                &mut board,
543                &mut pwm,
544                &enabled,
545                &[0.9, 0.9],
546                &[MixerOutputType::Motor, MixerOutputType::Aux],
547                &state
548            ),
549            Ok(true)
550        );
551        assert_eq!(pwm.send_count, 1);
552    }
553
554    #[test]
555    fn compose_pwm_outputs_preserves_primary_and_applies_aux_to_unused_channels() {
556        let mut params = Params::new();
557        params.set_by_id(ParamId::PARAM_MOTOR_IDLE_THROTTLE, ParamValue::Float(0.2));
558        params.set_by_id(ParamId::PARAM_SPIN_MOTORS_WHEN_ARMED, ParamValue::Int(1));
559        params.set_by_id(ParamId::PARAM_CHANNEL_OUTPUT_MASK, ParamValue::Int(-1));
560        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
561        let mut state = StateManager::new();
562        state.update(Event::INITIALIZED, &params);
563        state.update_arming_safety(true, true);
564        state.update(Event::REQUEST_ARM, &params);
565        let mut aux = RosflightAuxCmdMsg {
566            type_array: [RosflightAuxCmdType::Disabled; PWM_OUTPUT_CHANNELS],
567            aux_cmd_array: [0.0; PWM_OUTPUT_CHANNELS],
568        };
569        aux.type_array[4] = RosflightAuxCmdType::Servo;
570        aux.aux_cmd_array[4] = -0.5;
571        aux.type_array[5] = RosflightAuxCmdType::Motor;
572        aux.aux_cmd_array[5] = 0.1;
573
574        let output_types = [
575            MixerOutputType::Motor,
576            MixerOutputType::Motor,
577            MixerOutputType::Motor,
578            MixerOutputType::Motor,
579        ];
580        let outputs: [f64; PWM_OUTPUT_CHANNELS] = compose_pwm_outputs(
581            &[0.1, 0.2, 0.3, 0.4],
582            &output_types,
583            Some(&aux),
584            &state,
585            &params,
586        );
587
588        assert!((outputs[0] - 0.2).abs() < 1e-6);
589        assert!((outputs[1] - 0.2).abs() < 1e-6);
590        assert_eq!(outputs[2], 0.3);
591        assert_eq!(outputs[3], 0.4);
592        assert_eq!(outputs[4], 0.25);
593        assert!((outputs[5] - 0.2).abs() < 1e-6);
594        assert_eq!(outputs[6], 0.0);
595    }
596
597    #[test]
598    fn compose_pwm_outputs_forces_aux_motors_low_when_disarmed() {
599        let params = Params::new();
600        let mut state = StateManager::new();
601        state.update(Event::INITIALIZED, &params);
602        let mut aux = RosflightAuxCmdMsg {
603            type_array: [RosflightAuxCmdType::Disabled; PWM_OUTPUT_CHANNELS],
604            aux_cmd_array: [0.0; PWM_OUTPUT_CHANNELS],
605        };
606        aux.type_array[4] = RosflightAuxCmdType::Motor;
607        aux.aux_cmd_array[4] = 0.8;
608
609        let output_types = [
610            MixerOutputType::Motor,
611            MixerOutputType::Motor,
612            MixerOutputType::Motor,
613            MixerOutputType::Motor,
614        ];
615        let outputs: [f64; PWM_OUTPUT_CHANNELS] = compose_pwm_outputs(
616            &[0.1, 0.2, 0.3, 0.4],
617            &output_types,
618            Some(&aux),
619            &state,
620            &params,
621        );
622
623        assert_eq!(outputs[4], 0.0);
624        assert_eq!(outputs[6], 0.5);
625    }
626
627    #[test]
628    fn compose_pwm_outputs_applies_channel_output_mask_to_every_output_type() {
629        let mut params = Params::new();
630        params.set_by_id(ParamId::PARAM_MOTOR_IDLE_THROTTLE, ParamValue::Float(0.2));
631        params.set_by_id(ParamId::PARAM_SPIN_MOTORS_WHEN_ARMED, ParamValue::Int(1));
632        params.set_by_id(ParamId::PARAM_CHANNEL_OUTPUT_MASK, ParamValue::Int(0b0101));
633        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
634        let mut state = StateManager::new();
635        state.update(Event::INITIALIZED, &params);
636        state.update_arming_safety(true, true);
637        state.update(Event::REQUEST_ARM, &params);
638        let output_types = [
639            MixerOutputType::Motor,
640            MixerOutputType::Servo,
641            MixerOutputType::Gpio,
642            MixerOutputType::Aux,
643        ];
644
645        let outputs: [f64; PWM_OUTPUT_CHANNELS] =
646            compose_pwm_outputs(&[0.1, 0.4, 0.3, 0.5], &output_types, None, &state, &params);
647
648        assert!((outputs[0] - 0.2).abs() < 1e-6);
649        assert_eq!(outputs[1], 0.5);
650        assert_eq!(outputs[2], 1.0);
651        assert_eq!(outputs[3], 0.5);
652    }
653
654    #[test]
655    fn compose_pwm_outputs_default_channel_mask_enables_first_four_channels() {
656        let mut params = Params::new();
657        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
658        let mut state = StateManager::new();
659        state.update(Event::INITIALIZED, &params);
660        state.update_arming_safety(true, true);
661        state.update(Event::REQUEST_ARM, &params);
662        let output_types = [
663            MixerOutputType::Motor,
664            MixerOutputType::Servo,
665            MixerOutputType::Gpio,
666            MixerOutputType::Aux,
667        ];
668
669        let outputs: [f64; PWM_OUTPUT_CHANNELS] =
670            compose_pwm_outputs(&[0.1, 0.2, 0.3, 0.4], &output_types, None, &state, &params);
671
672        for (output, expected) in outputs.iter().zip([0.1, 0.6, 1.0, 0.0]) {
673            assert!((output - expected).abs() < 1e-6);
674        }
675    }
676
677    #[test]
678    fn compose_pwm_outputs_uses_aux_inside_primary_range_only_for_aux_owned_slots() {
679        let mut params = Params::new();
680        params.set_by_id(ParamId::PARAM_CHANNEL_OUTPUT_MASK, ParamValue::Int(-1));
681        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
682        let mut state = StateManager::new();
683        state.update(Event::INITIALIZED, &params);
684        state.update_arming_safety(true, true);
685        state.update(Event::REQUEST_ARM, &params);
686        let mut aux = RosflightAuxCmdMsg {
687            type_array: [RosflightAuxCmdType::Disabled; PWM_OUTPUT_CHANNELS],
688            aux_cmd_array: [0.0; PWM_OUTPUT_CHANNELS],
689        };
690        aux.type_array[1] = RosflightAuxCmdType::Servo;
691        aux.aux_cmd_array[1] = 1.0;
692        aux.type_array[2] = RosflightAuxCmdType::Servo;
693        aux.aux_cmd_array[2] = -1.0;
694        let output_types = [
695            MixerOutputType::Motor,
696            MixerOutputType::Aux,
697            MixerOutputType::Motor,
698            MixerOutputType::Motor,
699        ];
700
701        let outputs: [f64; PWM_OUTPUT_CHANNELS] = compose_pwm_outputs(
702            &[0.1, 0.2, 0.3, 0.4],
703            &output_types,
704            Some(&aux),
705            &state,
706            &params,
707        );
708
709        assert!((outputs[0] - 0.1).abs() < 1e-6);
710        assert_eq!(outputs[1], 1.0);
711        assert_eq!(outputs[2], 0.3);
712        assert_eq!(outputs[3], 0.4);
713    }
714
715    #[test]
716    fn compose_pwm_outputs_clamps_servo_gpio_and_motor_ranges() {
717        let mut params = Params::new();
718        params.set_by_id(ParamId::PARAM_CHANNEL_OUTPUT_MASK, ParamValue::Int(-1));
719        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
720        let mut state = StateManager::new();
721        state.update(Event::INITIALIZED, &params);
722        state.update_arming_safety(true, true);
723        state.update(Event::REQUEST_ARM, &params);
724        let output_types = [
725            MixerOutputType::Servo,
726            MixerOutputType::Servo,
727            MixerOutputType::Gpio,
728            MixerOutputType::Gpio,
729            MixerOutputType::Motor,
730            MixerOutputType::Motor,
731        ];
732
733        let outputs: [f64; PWM_OUTPUT_CHANNELS] = compose_pwm_outputs(
734            &[-2.0, 2.0, -0.1, 0.1, -0.5, 1.5],
735            &output_types,
736            None,
737            &state,
738            &params,
739        );
740
741        assert_eq!(outputs[0], 0.0);
742        assert_eq!(outputs[1], 1.0);
743        assert_eq!(outputs[2], 0.0);
744        assert_eq!(outputs[3], 1.0);
745        assert!((outputs[4] - 0.1).abs() < 1e-6);
746        assert_eq!(outputs[5], 1.0);
747    }
748}
veloxity_core/pwm/output_sync.rs

veloxity_core/pwm/
output_sync.rs
