From 8df09c9a7ce19c18189c8b4408e01ca34ab480fe Mon Sep 17 00:00:00 2001 From: pmarques-dev <72901351+pmarques-dev@users.noreply.github.com> Date: Mon, 5 Jun 2023 16:54:37 +0100 Subject: [PATCH] quadrature encoder: reduce PIO code size from 29 to 24 (#390) Co-authored-by: Paulo Marques --- pio/quadrature_encoder/quadrature_encoder.c | 8 +- pio/quadrature_encoder/quadrature_encoder.pio | 97 +++++++------------ 2 files changed, 42 insertions(+), 63 deletions(-) diff --git a/pio/quadrature_encoder/quadrature_encoder.c b/pio/quadrature_encoder/quadrature_encoder.c index a11ab89..1818a97 100644 --- a/pio/quadrature_encoder/quadrature_encoder.c +++ b/pio/quadrature_encoder/quadrature_encoder.c @@ -1,5 +1,5 @@ /** - * Copyright (c) 2021 pmarques-dev @ github + * Copyright (c) 2021-2023 pmarques-dev @ github * * SPDX-License-Identifier: BSD-3-Clause */ @@ -44,8 +44,10 @@ int main() { PIO pio = pio0; const uint sm = 0; - uint offset = pio_add_program(pio, &quadrature_encoder_program); - quadrature_encoder_program_init(pio, sm, offset, PIN_AB, 0); + // we don't really need to keep the offset, as this program must be loaded + // at offset 0 + pio_add_program(pio, &quadrature_encoder_program); + quadrature_encoder_program_init(pio, sm, PIN_AB, 0); while (1) { // note: thanks to two's complement arithmetic delta will always diff --git a/pio/quadrature_encoder/quadrature_encoder.pio b/pio/quadrature_encoder/quadrature_encoder.pio index d245d4b..63e1dfd 100644 --- a/pio/quadrature_encoder/quadrature_encoder.pio +++ b/pio/quadrature_encoder/quadrature_encoder.pio @@ -1,13 +1,12 @@ ; -; Copyright (c) 2021 pmarques-dev @ github +; Copyright (c) 2021-2023 pmarques-dev @ github ; ; SPDX-License-Identifier: BSD-3-Clause ; .program quadrature_encoder -; this code must be loaded into address 0, but at 29 instructions, it probably -; wouldn't be able to share space with other programs anyway +; the code must be loaded at address 0, because it uses computed jumps .origin 0 @@ -20,11 +19,11 @@ ; keeps the current encoder count and is incremented / decremented according to ; the steps sampled -; writing any non zero value to the TX FIFO makes the state machine push the -; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case -; sampling loop takes 14 cycles, so this program is able to read step rates up -; to sysclk / 14 (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec) - +; the program keeps trying to write the current count to the RX FIFO without +; blocking. To read the current count, the user code must drain the FIFO first +; and wait for a fresh sample (takes ~4 SM cycles on average). The worst case +; sampling loop takes 10 cycles, so this program is able to read step rates up +; to sysclk / 10 (e.g., sysclk 125MHz, max step rate = 12.5 Msteps/sec) ; 00 state JMP update ; read 00 @@ -60,41 +59,29 @@ decrement: ; this is where the main loop starts .wrap_target update: - ; we start by checking the TX FIFO to see if the main code is asking for - ; the current count after the PULL noblock, OSR will have either 0 if - ; there was nothing or the value that was there - SET X, 0 - PULL noblock - - ; since there are not many free registers, and PULL is done into OSR, we - ; have to do some juggling to avoid losing the state information and - ; still place the values where we need them - MOV X, OSR - MOV OSR, ISR - - ; the main code did not ask for the count, so just go to "sample_pins" - JMP !X, sample_pins - - ; if it did ask for the count, then we push it - MOV ISR, Y ; we trash ISR, but we already have a copy in OSR - PUSH + MOV ISR, Y + PUSH noblock sample_pins: ; we shift into ISR the last state of the 2 input pins (now in OSR) and ; the new state of the 2 pins, thus producing the 4 bit target for the - ; computed jump into the correct action for this state - MOV ISR, NULL - IN OSR, 2 + ; computed jump into the correct action for this state. Both the PUSH + ; above and the OUT below zero the other bits in ISR + OUT ISR, 2 IN PINS, 2 + + ; save the state in the OSR, so that we can use ISR for other purposes + MOV OSR, ISR + ; jump to the correct state machine action MOV PC, ISR ; the PIO does not have a increment instruction, so to do that we do a ; negate, decrement, negate sequence increment: - MOV X, !Y - JMP X--, increment_cont + MOV Y, ~Y + JMP Y--, increment_cont increment_cont: - MOV Y, !X + MOV Y, ~Y .wrap ; the .wrap here avoids one jump instruction and saves a cycle too @@ -106,16 +93,16 @@ increment_cont: // max_step_rate is used to lower the clock of the state machine to save power // if the application doesn't require a very high sampling rate. Passing zero -// will set the clock to the maximum, which gives a max step rate of around -// 8.9 Msteps/sec at 125MHz +// will set the clock to the maximum -static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate) +static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint pin, int max_step_rate) { pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false); gpio_pull_up(pin); gpio_pull_up(pin + 1); - pio_sm_config c = quadrature_encoder_program_get_default_config(offset); + pio_sm_config c = quadrature_encoder_program_get_default_config(0); + sm_config_set_in_pins(&c, pin); // for WAIT, IN sm_config_set_jmp_pin(&c, pin); // for JMP // shift to left, autopull disabled @@ -127,38 +114,28 @@ static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset if (max_step_rate == 0) { sm_config_set_clkdiv(&c, 1.0); } else { - // one state machine loop takes at most 14 cycles - float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate); + // one state machine loop takes at most 10 cycles + float div = (float)clock_get_hz(clk_sys) / (10 * max_step_rate); sm_config_set_clkdiv(&c, div); } - pio_sm_init(pio, sm, offset, &c); + pio_sm_init(pio, sm, 0, &c); pio_sm_set_enabled(pio, sm, true); } - -// When requesting the current count we may have to wait a few cycles (average -// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple -// encoders, we may request them all in one go and then fetch them all, thus -// avoiding doing the wait multiple times. If we are reading just one encoder, -// we can use the "get_count" function to request and wait - -static inline void quadrature_encoder_request_count(PIO pio, uint sm) -{ - pio->txf[sm] = 1; -} - -static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm) -{ - while (pio_sm_is_rx_fifo_empty(pio, sm)) - tight_loop_contents(); - return pio->rxf[sm]; -} - static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm) { - quadrature_encoder_request_count(pio, sm); - return quadrature_encoder_fetch_count(pio, sm); + uint ret; + int n; + + // if the FIFO has N entries, we fetch them to drain the FIFO, + // plus one entry which will be guaranteed to not be stale + n = pio_sm_get_rx_fifo_level(pio, sm) + 1; + while (n > 0) { + ret = pio_sm_get_blocking(pio, sm); + n--; + } + return ret; } %}