rktk_drivers_rp/rgb/
ws2812_pio.rs

1use core::convert::Infallible;
2
3use embassy_rp::dma::{AnyChannel, Channel};
4use embassy_rp::pio::{
5    Config, FifoJoin, Instance, Pio, PioPin, ShiftConfig, ShiftDirection, StateMachine,
6};
7use embassy_rp::{Peripheral, PeripheralRef, clocks, into_ref};
8use embassy_time::Timer;
9use fixed::types::U24F8;
10use fixed_macro::fixed;
11use rktk::drivers::interface::rgb::{LedRgb, RgbDriver};
12
13pub struct Ws2812Pio<'a, const MAX_LED_COUNT: usize, I: Instance> {
14    dma: PeripheralRef<'a, AnyChannel>,
15    sm: StateMachine<'a, I, 0>,
16}
17
18impl<'a, const MAX_LED_COUNT: usize, I: Instance> Ws2812Pio<'a, MAX_LED_COUNT, I> {
19    pub fn new<'b: 'a>(
20        mut pio: Pio<'a, I>,
21        data_pin: impl Peripheral<P = impl PioPin> + 'b,
22        dma: impl Peripheral<P = impl Channel> + 'a,
23    ) -> Self {
24        into_ref!(dma);
25
26        // Setup sm0
27
28        // prepare the PIO program
29        let side_set = pio::SideSet::new(false, 1, false);
30        let mut a: pio::Assembler<32> = pio::Assembler::new_with_side_set(side_set);
31
32        const T1: u8 = 2; // start bit
33        const T2: u8 = 5; // data bit
34        const T3: u8 = 3; // stop bit
35        const CYCLES_PER_BIT: u32 = (T1 + T2 + T3) as u32;
36
37        let mut wrap_target = a.label();
38        let mut wrap_source = a.label();
39        let mut do_zero = a.label();
40        a.set_with_side_set(pio::SetDestination::PINDIRS, 1, 0);
41        a.bind(&mut wrap_target);
42        // Do stop bit
43        a.out_with_delay_and_side_set(pio::OutDestination::X, 1, T3 - 1, 0);
44        // Do start bit
45        a.jmp_with_delay_and_side_set(pio::JmpCondition::XIsZero, &mut do_zero, T1 - 1, 1);
46        // Do data bit = 1
47        a.jmp_with_delay_and_side_set(pio::JmpCondition::Always, &mut wrap_target, T2 - 1, 1);
48        a.bind(&mut do_zero);
49        // Do data bit = 0
50        a.nop_with_delay_and_side_set(T2 - 1, 0);
51        a.bind(&mut wrap_source);
52
53        let prg = a.assemble_with_wrap(wrap_source, wrap_target);
54        let mut cfg = Config::default();
55
56        // Pin config
57        let out_pin = pio.common.make_pio_pin(data_pin);
58        cfg.set_out_pins(&[&out_pin]);
59        cfg.set_set_pins(&[&out_pin]);
60
61        cfg.use_program(&pio.common.load_program(&prg), &[&out_pin]);
62
63        // Clock config, measured in kHz to avoid overflows
64        // TODO CLOCK_FREQ should come from embassy_rp
65        let clock_freq = U24F8::from_num(clocks::clk_sys_freq() / 1000);
66        let ws2812_freq = fixed!(800: U24F8);
67        let bit_freq = ws2812_freq * CYCLES_PER_BIT;
68        cfg.clock_divider = clock_freq / bit_freq;
69
70        // FIFO config
71        cfg.fifo_join = FifoJoin::TxOnly;
72        cfg.shift_out = ShiftConfig {
73            auto_fill: true,
74            threshold: 24,
75            direction: ShiftDirection::Left,
76        };
77
78        pio.sm0.set_config(&cfg);
79        pio.sm0.set_enable(true);
80
81        Self {
82            dma: dma.map_into(),
83            sm: pio.sm0,
84        }
85    }
86}
87
88impl<const MAX_LED_COUNT: usize, I: Instance + 'static> RgbDriver
89    for Ws2812Pio<'static, MAX_LED_COUNT, I>
90{
91    type Error = Infallible;
92
93    async fn write<IT: IntoIterator<Item = LedRgb<u8>>>(
94        &mut self,
95        pixels: IT,
96    ) -> Result<(), Self::Error> {
97        let mut words = [0u32; MAX_LED_COUNT];
98        for (p, w) in pixels.into_iter().zip(words.iter_mut()) {
99            *w = (u32::from(p[1]) << 24) | (u32::from(p[0]) << 16) | (u32::from(p[2]) << 8);
100        }
101
102        // DMA transfer
103        self.sm
104            .tx()
105            .dma_push(self.dma.reborrow(), &words, false)
106            .await;
107
108        Timer::after_micros(55).await;
109
110        Ok(())
111    }
112}
rktk_drivers_rp/rgb/ws2812_pio.rs

rktk_drivers_rp/rgb/
ws2812_pio.rs
