Cameron/pico-examples
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

quadrature encoder: reduce PIO code size from 29 to 24 (#390)

Browse Source 
Co-authored-by: Paulo Marques <pm@quant-insight.com>
This commit is contained in:
pmarques-dev
2023-06-05 16:54:37 +01:00
committed by GitHub
parent 1c5d9aa567
commit 8df09c9a7c
2 changed files with 42 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
pio/quadrature_encoder/quadrature_encoder.c
View File

@@ -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

97
pio/quadrature_encoder/quadrature_encoder.pio
View File

@@ -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;
}
%}