Tidied up PIO implementation + commenting

drivers/servo/multi_pwm.cpp

@@ -5,8 +5,14 @@
 #include <vector>
 #include <algorithm>
 
+// Uncomment the below line to enable debugging
+// #define DEBUG_MULTI_PWM
+
 namespace servo {
 
+#ifdef DEBUG_MULTI_PWM
+    static const uint DEBUG_SIDESET = 17;
+#endif
 
 int data_dma_channel;
 int ctrl_dma_channel;
@@ -39,7 +45,6 @@ const bool use_loading_zone = true;
 
 uint irq_gpio = 15;
 uint write_gpio = 16;
-uint sideset_gpio = 17;
 
 void __isr pwm_dma_handler() {
     // Clear the interrupt request.
@@ -76,7 +81,11 @@ interrupt is fired, and the handler reconfigures channel A so that it is ready f
     
 
 MultiPWM::MultiPWM(PIO pio, uint sm, uint pin_mask) : pio(pio), sm(sm), pin_mask(pin_mask) {
+#ifdef DEBUG_MULTI_PWM
+    pio_program_offset = pio_add_program(pio, &debug_multi_pwm_program);
+#else
     pio_program_offset = pio_add_program(pio, &multi_pwm_program);
+#endif
 
     gpio_init(irq_gpio);
     gpio_set_dir(irq_gpio, GPIO_OUT);
@@ -97,12 +106,20 @@ MultiPWM::MultiPWM(PIO pio, uint sm, uint pin_mask) : pio(pio), sm(sm), pin_mask
         pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
     }
 
-    pio_gpio_init(pio, sideset_gpio);
-    pio_sm_set_consecutive_pindirs(pio, sm, sideset_gpio, 1, true);
+#ifdef DEBUG_MULTI_PWM
+    pio_gpio_init(pio, DEBUG_SIDESET);
+    pio_sm_set_consecutive_pindirs(pio, sm, DEBUG_SIDESET, 1, true);
+#endif
 
+#ifdef DEBUG_MULTI_PWM
+    pio_sm_config c = debug_multi_pwm_program_get_default_config(pio_program_offset);
+#else
     pio_sm_config c = multi_pwm_program_get_default_config(pio_program_offset);
+#endif
     sm_config_set_out_pins(&c, 0, irq_gpio); //TODO change this to be 32
-    sm_config_set_sideset_pins(&c, sideset_gpio);
+#ifdef DEBUG_MULTI_PWM
+    sm_config_set_sideset_pins(&c, DEBUG_SIDESET);
+#endif
     sm_config_set_out_shift(&c, false, true, 32);
     sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE); // We actively do not want a joined FIFO even though we are not needing the RX
 
@@ -185,7 +202,11 @@ MultiPWM::~MultiPWM() {
     dma_channel_unclaim(data_dma_channel);
     dma_channel_unclaim(ctrl_dma_channel);
     pio_sm_set_enabled(pio, sm, false);
+#ifdef DEBUG_MULTI_PWM
+    pio_remove_program(pio, &debug_multi_pwm_program, pio_program_offset);
+#else
     pio_remove_program(pio, &multi_pwm_program, pio_program_offset);
+#endif
 #ifndef MICROPY_BUILD_TYPE
     // pio_sm_unclaim seems to hardfault in MicroPython
     pio_sm_unclaim(pio, sm);

drivers/servo/multi_pwm.pio

@@ -1,24 +1,72 @@
+; --------------------------------------------------
+;              32-Channel PWM using PIO
+;      by Christopher (@ZodiusInfuser) Parrott
+; --------------------------------------------------
 ;
-; Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+; Outputs PWM signals to up to 32 pins using a single
+; state-machine. This is achieved by representing the
+; problem as a sequence of state changes and time delays.
 ;
-; SPDX-License-Identifier: BSD-3-Clause
+; This requires the use of DMA or a very fast loop to
+; provide data at a sufficient rate to prevent stalling.
 ;
+; The program pulls in two words:
+; - The first is a 32-bit pin mask of the states to
+;   change the pins to. Only the pins configured for
+;   use by this SM will be affected.
+; - The second is the time delay to wait before
+;   pulling in the next states.
+;
+; Each loop of the program takes 5 cycles, including an
+; initial 5 cycles when new data is loaded in. As such,
+; the time delay should be set to your "intended_delay - 1".
+;
+; To aid in debugging there is a variant program that uses
+; a sideset pin to show when new data is pulled in, as well
+; as when each loop has elapsed.These look like:
+;
+;      New Data                    Loop
+;     ._|‾|_._._                |‾.‾.‾.‾|_
+;      1 2 3 4 5                 1 2 3 4 5
 
+
+; Debounce Constants
+; --------------------------------------------------
+.define public MULT_PWM_CYCLES     5
+
+
+; PWM Program
+; --------------------------------------------------
 .program multi_pwm
-.side_set 1
 
 .wrap_target
-    ;pull side 0                    ; Pull in the next DWord containing the pin states, and time counter
-    ;out pins, 16 side 1            ; Immediately set the pins to their new state
-    ;out y, 16 [1] side 0           ; Set the counter
-
-    pull side 0                    ; Pull in the next DWord containing the pin states, and time counter
-    out pins, 32 side 1            ; Immediately set the pins to their new state
-    pull side 0
-    out y, 32 side 0           ; Set the counter
+    pull                    ; Pull in the new pin states
+    out pins, 32            ; Immediately set the pins to their new state
+    pull                    ; Pull in the delay counter
+    out y, 32               ; Set the counter
 count_check:
-    jmp y-- delay side 0           ; Check if the counter is 0, and if so wrap around. If not decrement the counter and jump to the delay
+    jmp y-- delay           ; Check if the counter is 0, and if so wrap around.
+                            ; If not decrement the counter and jump to the delay
 .wrap
 
 delay:
-    jmp count_check [3] side 1     ; Wait a few cycles then jump back to the loop
+    jmp count_check [3]     ; Wait a few cycles then jump back to the loop
+
+
+; Debug PWM Program
+; --------------------------------------------------
+.program debug_multi_pwm
+.side_set 1
+
+.wrap_target
+    pull side 0                     ; Pull in the new pin states
+    out pins, 32 side 1             ; Immediately set the pins to their new state
+    pull side 0                     ; Pull in the delay counter
+    out y, 32 side 0                ; Set the counter
+count_check:
+    jmp y-- delay side 0            ; Check if the counter is 0, and if so wrap around.
+                                    ;If not decrement the counter and jump to the delay
+.wrap
+
+delay:
+    jmp count_check [3] side 1      ; Wait a few cycles then jump back to the loop