frc971/imu/quadrature_encoder.pio

;
; Copyright (c) 2021 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
.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

; 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)


; 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 X--" 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:
	; 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

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
	IN PINS, 2
	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
increment_cont:
	MOV Y, !X
.wrap	; the .wrap here avoids one jump instruction and saves a cycle too



% c-sdk {

#include "hardware/clocks.h"
#include "hardware/gpio.h"

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

static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate)
{
	pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false);
	pio_gpio_init(pio, pin);
	gpio_pull_up(pin);

	pio_sm_config c = quadrature_encoder_program_get_default_config(offset);
	sm_config_set_in_pins(&c, pin); // for WAIT, IN
	sm_config_set_jmp_pin(&c, pin); // for JMP
	// shift to left, autopull disabled
	sm_config_set_in_shift(&c, false, false, 32);
	// don't join FIFO's
	sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);

	// passing "0" as the sample frequency,
	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);
		sm_config_set_clkdiv(&c, div);
	}

	pio_sm_init(pio, sm, offset, &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);
}

%}