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

veloxity_core/pwm/
output_sync.rs
