jepler/Adafruit_Blinka_rp1pio
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Add more bits, enough to support a pio-coded quadrature counter

Browse source
This commit is contained in:
Jeff Epler 2025-01-28 12:09:35 -06:00
parent 975adae0b6
commit e37ed4d291
2 changed files with 370 additions and 16 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

150
examples/quadrature_encoder.py Normal file
View file

@ -0,0 +1,150 @@
import array
import sys
import adafruit_pioasm
if sys.implementation.name == 'circuitpython':
from rp2pio import StateMachine
_n_read = 9
else:
from adafruit_rp1pio import StateMachine
_n_read = 17
_program = adafruit_pioasm.Program("""
;
; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
;
; SPDX-License-Identifier: BSD-3-Clause
;
.pio_version 0 // only requires PIO version 0
.program quadrature_encoder
; the code must be loaded at address 0, because it uses computed jumps
.origin 0
; the code works by running a loop that continuously shifts the 2 phase pins into
; ISR and looks at the lower 4 bits to do a computed jump to an instruction that
; does the proper "do nothing" | "increment" | "decrement" action for that pin
; state change (or no change)
; ISR holds the last state of the 2 pins during most of the code. The Y register
; keeps the current encoder count and is incremented / decremented according to
; the steps sampled
; 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
jmp decrement ; read 01
jmp increment ; read 10
jmp update ; read 11
; 01 state
jmp increment ; read 00
jmp update ; read 01
jmp update ; read 10
jmp decrement ; read 11
; 10 state
jmp decrement ; read 00
jmp update ; read 01
jmp update ; read 10
jmp increment ; read 11
; to reduce code size, the last 2 states are implemented in place and become the
; target for the other jumps
; 11 state
jmp update ; read 00
jmp increment ; read 01
decrement:
; note: the target of this instruction must be the next address, so that
; the effect of the instruction does not depend on the value of Y. The
; same is true for the "jmp y--" below. Basically "jmp y--, <next addr>"
; is just a pure "decrement y" instruction, with no other side effects
jmp y--, update ; read 10
; this is where the main loop starts
.wrap_target
update:
mov isr, y ; read 11
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. Both the PUSH
; above and the OUT below zero out 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 y, ~y
jmp y--, increment_cont
increment_cont:
mov y, ~y
.wrap ; the .wrap here avoids one jump instruction and saves a cycle too
""")
_zero_y = adafruit_pioasm.assemble("set y 0")
class IncrementalEncoder:
def __init__(self, pin_a, pin_b=None, divisor=4):
"""Create an incremental encoder on pin_a and the next higher pin
Always operates in "x4" mode (one count per quadrature edge)
Assumes but does not check that pin_b is one above pin_a."""
self._sm = StateMachine(
_program.assembled,
frequency=0,
init=_zero_y,
first_in_pin=pin_a,
in_pin_count=2,
pull_in_pin_up=0x3,
auto_push=True,
push_threshold=32,
in_shift_right=False,
**_program.pio_kwargs
)
self._buffer = array.array('i',[0] * _n_read)
self.divisor = divisor
self._position = 0
def deinit(self):
self._sm.deinit()
@property
def position(self):
self._sm.readinto(self._buffer) # read N stale values + 1 fresh value
raw_position = self._buffer[-1]
delta = int((raw_position - self._position * self.divisor) / self.divisor)
self._position += delta
return self._position
if __name__ == '__main__':
import board
# D17/D18 on header pins 11/12
# GND on header pin 6/9
# +5V on header pins 2/4
q = IncrementalEncoder(board.D17)
old_position = q.position
while True:
position = q.position
if position != old_position:
delta = position - old_position
print(f"{position:8d} {delta=}")
old_position = position

236
src/main.cpp
View file

@ -11,6 +11,38 @@ namespace py = pybind11;
namespace {
typedef struct { uint32_t value;
} pio_pinmask_t;
typedef uint32_t pio_pinmask_value_t;
#define PIO_PINMASK_C(c) UINT32_C(c)
#define PIO_PINMASK_BIT (32)
#define PIO_PINMASK(i) (UINT32_C(1) << (i))
#define PIO_PINMASK_PRINT(p) mp_printf(&mp_plat_print, "%s:%d: %s = %08x\n", \
__FILE__, __LINE__, #p, \
(uint32_t)(PIO_PINMASK_VALUE(p)));
#define PIO_PINMASK_ALL PIO_PINMASK_FROM_VALUE(~UINT32_C(0))
#define PIO_PINMASK_VALUE(p) ((p).value)
#define PIO_PINMASK_FROM_VALUE(v) ((pio_pinmask_t) {(v)})
#define PIO_PINMASK_FROM_PIN(i) ((pio_pinmask_t) {(PIO_PINMASK(i))})
#define PIO_PINMASK_NONE PIO_PINMASK_FROM_VALUE(0)
#define PIO_PINMASK_SET(p, i) ((p).value |= PIO_PINMASK(i))
#define PIO_PINMASK_CLEAR(p, i) ((p).value &= ~PIO_PINMASK(i))
#define PIO_PINMASK_IS_SET(p, i) (((p).value & PIO_PINMASK(i)) != 0)
#define PIO_PINMASK_BINOP(op, p, q) PIO_PINMASK_FROM_VALUE((p).value op(q).value)
#define PIO_PINMASK_BINOP_ASSIGN(op, p, q) ((p).value op(q).value)
#define PIO_PINMASK_EQUAL(p, q) ((p).value == (q).value)
#define PIO_PINMASK_AND(p, q) PIO_PINMASK_BINOP(&, (p), (q))
#define PIO_PINMASK_AND_NOT(p, q) PIO_PINMASK_BINOP(&~, (p), (q))
#define PIO_PINMASK_OR(p, q) PIO_PINMASK_BINOP(|, (p), (q))
#define PIO_PINMASK_OR3(p, q, r) PIO_PINMASK_OR((p), PIO_PINMASK_OR((q), (r)))
#define PIO_PINMASK_INTERSECT(p, q) PIO_PINMASK_BINOP_ASSIGN( &=, (p), (q))
#define PIO_PINMASK_DIFFERENCE(p, q) PIO_PINMASK_BINOP_ASSIGN( &= ~, (p), (q))
#define PIO_PINMASK_MERGE(p, q) PIO_PINMASK_BINOP_ASSIGN( |=, (p), (q))
#define PIO_PINMASK_CONSECUTIVE_PINS(start, count) PIO_PINMASK_FROM_VALUE(((PIO_PINMASK_C(1) << count) - 1) << start)
#define PIO_PINMASK_SHIFTED(p, count) PIO_PINMASK_FROM_VALUE(PIO_PINMASK_FROM_VALUE(p) << count)
PIO pio_open_check() {
if (pio_init()) {
throw std::runtime_error("PIO not available");
@ -44,6 +76,21 @@ int get_pin_number(py::object gpio_obj) {
return py::getattr(gpio_obj, "_pin", gpio_obj).attr("id").cast<int>();
}
#ifndef NUM_BANK0_GPIOS
#define NUM_BANK0_GPIOS (28)
#endif
static void rp2pio_statemachine_set_pull(pio_pinmask_t pull_pin_up, pio_pinmask_t pull_pin_down, pio_pinmask_t pins_we_use) {
for (size_t i = 0; i < NUM_BANK0_GPIOS; i++) {
bool used = PIO_PINMASK_IS_SET(pins_we_use, i);
if (used) {
bool pull_up = PIO_PINMASK_IS_SET(pull_pin_up, i);
bool pull_down = PIO_PINMASK_IS_SET(pull_pin_down, i);
gpio_set_pulls(i, pull_up, pull_down);
}
}
}
template<class T>
int get_default(py::object o, T default_value) {
if (o.is_none()) { return default_value; }
@ -58,44 +105,112 @@ class StateMachine {
public:
StateMachine(py::buffer assembled,
double frequency,
int8_t offset,
py::buffer init,
py::object first_sideset_pin,
int sideset_pin_count,
bool sideset_enable,
py::object first_in_pin,
int in_pin_count,
uint32_t pull_in_pin_up,
uint32_t pull_in_pin_down,
bool auto_pull,
bool out_shift_right,
int pull_threshold) {
int pull_threshold,
bool auto_push,
bool in_shift_right,
int push_threshold,
int wrap,
int wrap_target) {
pio = pio_open_check();
sm = pio_sm_claim_unused_sm_check(pio);
py::buffer_info info = assembled.request();
if (info.itemsize != 2) {
throw py::value_error("assembled: Expected array of type `h`");
throw py::value_error("assembled: Expected array of type `H`");
}
if (info.size >= 32) {
throw py::value_error("assembled: Exceeds maximum program length (32)");
}
if (offset < -1 || offset > 31) {
throw py::value_error("offset exceeds valid range of -1 to 31 inclusive");
}
if (!init.is_none()) {
py::buffer_info init_info = init.request();
if (info.itemsize != 2) {
throw py::value_error("init: Expected array of type `H`");
}
}
ssize_t program_len = info.size;
if(wrap == -1) {
wrap = program_len - 1;
}
if(wrap < 0 || wrap >= program_len) {
throw py::value_error("wrap out of range");
}
if(wrap_target < 0 || wrap >= program_len) {
throw py::value_error("wrap_target out of range");
}
pio_pinmask_t pindirs = PIO_PINMASK_NONE;
pio_pinmask_t pins_we_use = PIO_PINMASK_NONE;
pio_pinmask_t pin_pull_up = PIO_PINMASK_NONE;
pio_pinmask_t pin_pull_down = PIO_PINMASK_NONE;
struct pio_program program = {
.instructions = reinterpret_cast<uint16_t*>(info.ptr),
.length = static_cast<uint8_t>(info.size),
.origin = -1,
.origin = offset,
};
offset = pio_add_program_check(pio, &program);
pio_sm_config c = {0, 0, 0};
sm_config_set_wrap(&c, offset, offset + info.size - 1);
offset = pio_add_program_check(pio, &program);
wrap += offset;
wrap_target += offset;
pio_sm_config c = pio_get_default_sm_config();
sm_config_set_wrap(&c, wrap_target, wrap);
if (!first_sideset_pin.is_none()) {
if (sideset_pin_count < 1 || sideset_pin_count > 5) {
throw py::value_error("sideset_pin_count out of range (must be 1 to 5)");
}
auto first_sideset_pin_number = get_pin_number(first_sideset_pin);
if (sideset_pin_count < 1 || (sideset_enable + sideset_pin_count) > 5 || first_sideset_pin_number + sideset_pin_count > NUM_BANK0_GPIOS) {
throw py::value_error("sideset_pin_count out of range");
}
PIO_PINMASK_MERGE(pindirs, PIO_PINMASK_CONSECUTIVE_PINS(sideset_pin_count, first_sideset_pin_number));
PIO_PINMASK_MERGE(pins_we_use, PIO_PINMASK_CONSECUTIVE_PINS(sideset_pin_count, first_sideset_pin_number));
for(int i=0; i<sideset_pin_count; i++) {
pio_gpio_init(pio, first_sideset_pin_number + i);
}
sm_config_set_sideset(&c, sideset_pin_count, /* optional */ false, /* pindirs */ false);
sm_config_set_sideset(&c, sideset_pin_count + sideset_enable, /* optional */ sideset_enable, /* pindirs */ false);
sm_config_set_sideset_pins(&c, first_sideset_pin_number);
}
if (!first_in_pin.is_none()) {
auto first_in_pin_number = get_pin_number(first_in_pin);
if (in_pin_count < 1 || first_in_pin_number + in_pin_count > NUM_BANK0_GPIOS) {
throw py::value_error("sideset_pin_count out of range");
}
for(int i=0; i<in_pin_count; i++) {
pio_gpio_init(pio, first_in_pin_number + i);
}
sm_config_set_in_pins(&c, first_in_pin_number);
PIO_PINMASK_MERGE(pin_pull_up, PIO_PINMASK_FROM_VALUE(pull_in_pin_up << first_in_pin_number));
PIO_PINMASK_MERGE(pin_pull_down, PIO_PINMASK_FROM_VALUE(pull_in_pin_down << first_in_pin_number));
PIO_PINMASK_MERGE(pins_we_use, PIO_PINMASK_CONSECUTIVE_PINS(in_pin_count, first_in_pin_number));
}
pio_sm_set_pindirs_with_mask(pio, sm, PIO_PINMASK_VALUE(pindirs), PIO_PINMASK_VALUE(pins_we_use));
rp2pio_statemachine_set_pull(pin_pull_up, pin_pull_down, pins_we_use);
if (!init.is_none()) {
run(init);
}
sm_config_set_out_shift(&c, out_shift_right, auto_pull, pull_threshold);
sm_config_set_in_shift(&c, in_shift_right, auto_push, push_threshold);
double div = frequency ? clock_get_hz(clk_sys) / frequency : 1.0;
sm_config_set_clkdiv(&c, div);
@ -103,12 +218,78 @@ public:
pio_sm_init(pio, sm, offset, &c);
pio_sm_set_enabled(pio, sm, true);
if (pio_sm_config_xfer(pio, sm, PIO_DIR_TO_SM, 65532, 2)) {
throw std::runtime_error("pio_sm_config_xfer(PIO_DIR_TO_SM) failed");
}
if (pio_sm_config_xfer(pio, sm, PIO_DIR_FROM_SM, 65532, 2)) {
throw std::runtime_error("pio_sm_config_xfer(PIO_DIR_FROM_SM) failed");
}
}
void run(py::buffer instructions) {
py::buffer_info info = instructions.request();
if (info.itemsize != 2) {
throw py::value_error("instructions: Expected array of type `H`");
}
auto raw_instructions = reinterpret_cast<const uint16_t *>(info.ptr);
for (ssize_t i = 0; i < info.size; i++) {
pio_sm_exec(pio, sm, raw_instructions[i]);
}
}
~StateMachine() {
if(!PIO_IS_ERR(pio)) pio_close(pio);
}
void readinto(py::buffer b) {
py::buffer_info info = b.request();
uint32_t *info_ptr32 = reinterpret_cast<uint32_t*>(info.ptr);
uint32_t *ptr = info_ptr32;
if (info.readonly) {
throw py::type_error("read-only buffer");
}
std::vector<uint32_t> vec;
// the DMA controller doesn't replicate 8- and 16-bit values like on rp2, so we have to do it ourselves
if (info.itemsize != 4) {
vec.reserve(info.size);
switch(info.itemsize) {
case 1:
break;
case 2:
break;
default:
throw py::value_error("buffer must contain items of 1, 2, or 4 bytes");
}
ptr = &vec[0];
}
size_t size = info.size * sizeof(uint32_t);
if (pio_sm_xfer_data(pio, sm, PIO_DIR_FROM_SM, size, ptr)) {
throw std::runtime_error("pio_sm_xfer_data() failed");
}
switch(info.itemsize) {
case 1:
{
uint8_t *info_ptr8 = reinterpret_cast<uint8_t*>(info.ptr);
for(ssize_t i=0; i<info.size; i++) {
info_ptr8[i] = vec[i];
}
}
break;
case 2:
{
uint16_t *info_ptr16 = reinterpret_cast<uint16_t*>(info.ptr);
for(ssize_t i=0; i<info.size; i++) {
info_ptr16[i] = vec[i];
}
}
break;
}
}
void write(py::buffer b) {
py::buffer_info info = b.request();
uint32_t *ptr = reinterpret_cast<uint32_t*>(info.ptr);
@ -139,9 +320,6 @@ public:
ptr = &vec[0];
}
size_t size = info.size * sizeof(uint32_t);
if (pio_sm_config_xfer(pio, sm, PIO_DIR_TO_SM, size, 1)) {
throw std::runtime_error("pio_sm_config_xfer() failed");
}
if (pio_sm_xfer_data(pio, sm, PIO_DIR_TO_SM, size, ptr)) {
throw std::runtime_error("pio_sm_xfer_data() failed");
}
@ -168,22 +346,48 @@ PYBIND11_MODULE(adafruit_rp1pio, m) {
py::class_<StateMachine>(m, "StateMachine", "A single PIO StateMachine")
.def(py::init<py::buffer /* assembled */,
double /* frequency */,
int8_t /* offset */,
py::buffer /* init */,
py::object /* first_sideset_pin */,
int /* sideset_pin_count */,
bool /* sideset_enable */,
py::object /* first_in_pin */,
int /* in_pin_count */,
uint32_t /* pin_in_pull_up */,
uint32_t /* pin_in_pull_down */,
bool /* auto_pull */,
bool /* out_shift_right */,
int /* pull_threshold */>(),
int /* pull_threshold */,
bool /* auto_push */,
bool /* in_shift_right */,
int /* push_threshold */,
int /* wrap */,
int /* wrap_target */ >(),
"Construct a StateMachine",
py::arg("assembled"),
py::arg("frequency"),
py::kw_only(),
py::arg("offset") = -1,
py::arg("init") = py::none(),
py::arg("first_sideset_pin") = py::none(),
py::arg("sideset_pin_count") = 1,
py::arg("sideset_enable") = false,
py::arg("first_in_pin") = py::none(),
py::arg("in_pin_count") = 1,
py::arg("pull_in_pin_up") = 0,
py::arg("pull_in_pin_down") = 0,
py::arg("auto_pull") = false,
py::arg("out_shift_right") = true,
py::arg("pull_threshold") = 32
py::arg("pull_threshold") = 32,
py::arg("auto_push") = false,
py::arg("in_shift_right") = true,
py::arg("push_threshold") = 32,
py::arg("wrap") = -1,
py::arg("wrap_target") = 0
)
.def("write", &StateMachine::write, "Write the data contained in buffer to the state machine", py::arg("buffer"));
.def("write", &StateMachine::write, "Write the data contained in buffer to the state machine", py::arg("buffer"))
.def("readinto", &StateMachine::readinto, "Read data from the state machine into a buffer", py::arg("buffer"))
.def("run", &StateMachine::run, "Execute instructions on the state machine", py::arg("instructions"));
#ifdef VERSION_INFO
m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO);